Merge old branch states-path-finder.

CMakeLists.txt

@@ -86,6 +86,8 @@ set(${PROJECT_NAME}_HEADERS
     include/hpp/manipulation/path-optimization/random-shortcut.hh
     include/hpp/manipulation/path-optimization/spline-gradient-based.hh
     include/hpp/manipulation/path-planner/end-effector-trajectory.hh
+    include/hpp/manipulation/path-planner/in-state-path.hh
+    include/hpp/manipulation/path-planner/states-path-finder.hh
     include/hpp/manipulation/problem-target/state.hh
     include/hpp/manipulation/serialization.hh
     include/hpp/manipulation/steering-method/cross-state-optimization.hh

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

@@ -95,6 +95,14 @@ typedef core::vectorIn_t vectorIn_t;
 typedef core::vectorOut_t vectorOut_t;
 HPP_PREDEF_CLASS(ManipulationPlanner);
 typedef shared_ptr<ManipulationPlanner> ManipulationPlannerPtr_t;
+namespace pathPlanner {
+  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;
+          
+          /// 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;
+
+          /// 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();
+
+          virtual void startSolve();
+          virtual void oneStep();
+          /// Do nothing
+          virtual void tryConnectInitAndGoals ();
+
+        protected:
+          StatesPathFinder (const core::ProblemConstPtr_t& problem,
+                            const core::RoadmapPtr_t&);
+          StatesPathFinder (const StatesPathFinder& other);
+
+          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 getConfigList () 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_;
+          /// Path planning problem in each leaf.
+          core::ProblemPtr_t inStateProblem_;
+
+          /// Vector of parameterizable edge numerical constraints
+          NumericalConstraints_t constraints_;
+          /// Map of indices 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_;
+          /// Index of the sequence of states
+          std::size_t idxSol_ = 0;
+          graph::Edges_t lastBuiltTransitions_;
+
+          bool skipColAnalysis_;
+
+          // Constraints defining the goal
+          NumericalConstraints_t goalConstraints_;
+          // Variables used across several calls to oneStep
+          ConfigurationPtr_t q1_, q2_;
+          core::Configurations_t configList_;
+          std::size_t idxConfigList_;
+          size_type nTryConfigList_;
+          bool solved_, interrupt_;
+
+          /// Weak pointer to itself
+          StatesPathFinderWkPtr_t weak_;
+
+      }; // class StatesPathFinder
+      /// \}
+      
+    } // namespace pathPlanner
+  } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH

src/problem-solver.cc

@@ -62,6 +62,8 @@
 #include "hpp/manipulation/path-optimization/enforce-transition-semantic.hh"
 #include "hpp/manipulation/path-optimization/random-shortcut.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/problem.hh"
 #include "hpp/manipulation/roadmap.hh"
@@ -123,7 +125,8 @@ ProblemSolver::ProblemSolver() : core::ProblemSolver(), robot_(), problem_() {
   pathPlanners.add("M-RRT", ManipulationPlanner::create);
   pathPlanners.add("EndEffectorTrajectory",
                    pathPlanner::EndEffectorTrajectory::createWithRoadmap);
-
+  pathPlanners.add ("StatesPathFinder",
+                    pathPlanner::StatesPathFinder::createWithRoadmap);
   pathValidations.add("Graph-Discretized",
                       createDiscretizedCollisionGraphPathValidation);
   pathValidations.add("Graph-DiscretizedCollision",