diff --git a/CMakeLists.txt b/CMakeLists.txt
index f86a3ac607446d1d51e344d3486ce5d4e8b44cd4..02722087bfae59b594299242b5683fe4fb30d91b 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/NEWS b/NEWS
index 2c7e97af0f3001b48a3f404f09c6563be6edc782..1004ee6a703869421e14f454a3724a9378880841 100644
--- a/NEWS
+++ b/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.
diff --git a/TODO.txt b/TODO.txt
new file mode 100644
index 0000000000000000000000000000000000000000..dd8d56b526651ed184ef92aac47abff26f00f3fa
--- /dev/null
+++ b/TODO.txt
@@ -0,0 +1,25 @@
+- 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.
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 960121443a9fe60fc7e010868b9d1e3003a85463..ba55b2ad6d2b05a972d9792fa0a8f94a3d400e74 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/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);
diff --git a/include/hpp/manipulation/path-planner/in-state-path.hh b/include/hpp/manipulation/path-planner/in-state-path.hh
new file mode 100644
index 0000000000000000000000000000000000000000..4395ed6daff7b459eda72c9a29ffb70e472ac946
--- /dev/null
+++ b/include/hpp/manipulation/path-planner/in-state-path.hh
@@ -0,0 +1,135 @@
+// 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
diff --git a/include/hpp/manipulation/path-planner/states-path-finder.hh b/include/hpp/manipulation/path-planner/states-path-finder.hh
new file mode 100644
index 0000000000000000000000000000000000000000..288414d1ba3b938ef5c8f04cfeffb3e81857c8e4
--- /dev/null
+++ b/include/hpp/manipulation/path-planner/states-path-finder.hh
@@ -0,0 +1,252 @@
+// 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
diff --git a/include/hpp/manipulation/steering-method/cross-state-optimization.hh b/include/hpp/manipulation/steering-method/cross-state-optimization.hh
index 376515b75ca03b34420fefe1c912d03082431002..be761f86a027968642cbbbab2f893db79b0ea312 100644
--- a/include/hpp/manipulation/steering-method/cross-state-optimization.hh
+++ b/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
diff --git a/src/graph-optimizer.cc b/src/graph-optimizer.cc
index 62ca44cc571f0b73daf0652adc39654594b2202b..f2c26038bd41e0e019cef2d5ee4d2dd108451f8f 100644
--- a/src/graph-optimizer.cc
+++ b/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 ());
diff --git a/src/graph-path-validation.cc b/src/graph-path-validation.cc
index e02e8eaf0eb503dd0852be45c4f51460d7d862d5..f10ac3124509d076dbae4aa4d09d99bd7b484852 100644
--- a/src/graph-path-validation.cc
+++ b/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 {
diff --git a/src/graph/state.cc b/src/graph/state.cc
index 62915b1bda4946c08b5b731a544881c2d2e111fc..b527334e2f0c31036c9ffa08b29f6bb1617a2cf2 100644
--- a/src/graph/state.cc
+++ b/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);
}
}
diff --git a/src/path-planner/in-state-path.cc b/src/path-planner/in-state-path.cc
new file mode 100644
index 0000000000000000000000000000000000000000..728eca1b1b69f3124caf909524b07a950a109aab
--- /dev/null
+++ b/src/path-planner/in-state-path.cc
@@ -0,0 +1,232 @@
+// 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
diff --git a/src/path-planner/states-path-finder.cc b/src/path-planner/states-path-finder.cc
new file mode 100644
index 0000000000000000000000000000000000000000..fd0cf507826f11d8a389e7f9070d93c74f6eb65c
--- /dev/null
+++ b/src/path-planner/states-path-finder.cc
@@ -0,0 +1,1332 @@
+// 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/states-path-finder.hh>
+
+#include <map>
+#include <queue>
+#include <vector>
+
+#include <hpp/util/exception-factory.hh>
+#include <hpp/util/timer.hh>
+
+#include <pinocchio/multibody/model.hpp>
+
+#include <hpp/pinocchio/configuration.hh>
+#include <hpp/pinocchio/joint-collection.hh>
+
+#include <hpp/constraints/affine-function.hh>
+#include <hpp/constraints/locked-joint.hh>
+#include <hpp/constraints/solver/by-substitution.hh>
+#include <hpp/constraints/explicit.hh>
+
+#include <hpp/core/path-vector.hh>
+#include <hpp/core/configuration-shooter.hh>
+#include <hpp/core/collision-validation.hh>
+#include <hpp/core/collision-validation-report.hh>
+
+#include <hpp/manipulation/graph/edge.hh>
+#include <hpp/manipulation/graph/state.hh>
+#include <hpp/manipulation/roadmap.hh>
+
+#include <hpp/manipulation/path-planner/in-state-path.hh>
+
+namespace hpp {
+ namespace manipulation {
+ namespace pathPlanner {
+
+ using Eigen::RowBlockIndices;
+ using Eigen::ColBlockIndices;
+
+ using graph::StatePtr_t;
+ using graph::States_t;
+ using graph::EdgePtr_t;
+ using graph::Edges_t;
+ using graph::Neighbors_t;
+ using graph::NumericalConstraints_t;
+ using graph::LockedJoints_t;
+ using graph::segments_t;
+
+
+ StatesPathFinderPtr_t StatesPathFinder::create (
+ const core::ProblemConstPtr_t& problem)
+ {
+ StatesPathFinder* ptr;
+ RoadmapPtr_t r = Roadmap::create(problem->distance(), problem->robot());
+ try {
+ ProblemConstPtr_t p(HPP_DYNAMIC_PTR_CAST(const Problem, problem));
+ ptr = new StatesPathFinder (p, r);
+ } catch (std::exception&) {
+ throw std::invalid_argument
+ ("The problem must be of type hpp::manipulation::Problem.");
+ }
+ StatesPathFinderPtr_t shPtr (ptr);
+ ptr->init (shPtr);
+ return shPtr;
+ }
+
+ StatesPathFinderPtr_t StatesPathFinder::createWithRoadmap (
+ const core::ProblemConstPtr_t& problem,
+ const core::RoadmapPtr_t& roadmap)
+ {
+ StatesPathFinder* ptr;
+ core::RoadmapPtr_t r2 = roadmap; // unused
+ RoadmapPtr_t r;
+ try {
+ ProblemConstPtr_t p(HPP_DYNAMIC_PTR_CAST(const Problem, problem));
+ r = HPP_DYNAMIC_PTR_CAST (Roadmap, r2);
+ ptr = new StatesPathFinder (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.");
+ }
+ StatesPathFinderPtr_t shPtr (ptr);
+ ptr->init (shPtr);
+ return shPtr;
+ }
+
+ StatesPathFinderPtr_t StatesPathFinder::copy () const
+ {
+ StatesPathFinder* ptr = new StatesPathFinder (*this);
+ StatesPathFinderPtr_t shPtr (ptr);
+ ptr->init (shPtr);
+ return shPtr;
+ }
+
+ struct StatesPathFinder::GraphSearchData
+ {
+ StatePtr_t s1, s2;
+
+ // Datas for findNextTransitions
+ struct state_with_depth {
+ StatePtr_t s;
+ EdgePtr_t e;
+ std::size_t l; // depth to root
+ std::size_t i; // index in parent state_with_depths_t
+ inline state_with_depth () : s(), e(), l(0), i (0) {}
+ inline state_with_depth (EdgePtr_t _e, std::size_t _l, std::size_t _i)
+ : s(_e->stateFrom()), e(_e), l(_l), i (_i) {}
+ };
+ typedef std::vector<state_with_depth> state_with_depths_t;
+ typedef std::map<StatePtr_t,state_with_depths_t> StateMap_t;
+ /// std::size_t is the index in state_with_depths_t at StateMap_t::iterator
+ struct state_with_depth_ptr_t {
+ StateMap_t::iterator state;
+ std::size_t parentIdx;
+ state_with_depth_ptr_t (const StateMap_t::iterator& it, std::size_t idx) : state (it), parentIdx (idx) {}
+ };
+ typedef std::queue<state_with_depth_ptr_t> Queue_t;
+ std::size_t maxDepth;
+ StateMap_t parent1;
+ Queue_t queue1;
+
+ const state_with_depth& getParent(const state_with_depth_ptr_t& _p) const
+ {
+ const state_with_depths_t& parents = _p.state->second;
+ return parents[_p.parentIdx];
+ }
+
+ state_with_depth_ptr_t addInitState()
+ {
+ StateMap_t::iterator next =
+ parent1.insert(StateMap_t::value_type(s1, state_with_depths_t(1))).first;
+ return state_with_depth_ptr_t (next, 0);
+ }
+
+ state_with_depth_ptr_t addParent(
+ const state_with_depth_ptr_t& _p,
+ const EdgePtr_t& transition)
+ {
+ const state_with_depths_t& parents = _p.state->second;
+ const state_with_depth& from = parents[_p.parentIdx];
+
+ // Insert state to if necessary
+ StateMap_t::iterator next = parent1.insert (
+ StateMap_t::value_type(
+ transition->stateTo(), state_with_depths_t ()
+ )).first;
+
+ next->second.push_back (
+ state_with_depth(transition, from.l + 1, _p.parentIdx));
+
+ return state_with_depth_ptr_t (next, next->second.size()-1);
+ }
+ };
+
+ static bool containsLevelSet(const graph::EdgePtr_t& e) {
+ 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;
+ }
+
+ void StatesPathFinder::gatherGraphConstraints ()
+ {
+ typedef graph::Edge Edge;
+ typedef graph::EdgePtr_t EdgePtr_t;
+ typedef graph::GraphPtr_t GraphPtr_t;
+ typedef constraints::solver::BySubstitution Solver_t;
+
+ GraphPtr_t cg (problem_->constraintGraph ());
+ const ConstraintsAndComplements_t& cac
+ (cg->constraintsAndComplements ());
+ for (std::size_t i = 0; i < cg->nbComponents (); ++i) {
+ EdgePtr_t edge (HPP_DYNAMIC_PTR_CAST (Edge, cg->get (i).lock ()));
+ if (edge) {
+ // Don't even consider level set edges
+ if (containsLevelSet(edge)) continue;
+ const Solver_t& solver (edge->pathConstraint ()->
+ configProjector ()->solver ());
+ const NumericalConstraints_t& constraints
+ (solver.numericalConstraints ());
+ for (NumericalConstraints_t::const_iterator it
+ (constraints.begin ()); it != constraints.end (); ++it) {
+ if ((*it)->parameterSize () > 0) {
+ const std::string& name ((*it)->function ().name ());
+ if (index_.find (name) == index_.end ()) {
+ // constraint is not in map, add it
+ index_ [name] = constraints_.size ();
+ // Check whether constraint is equivalent to a previous one
+ for (NumericalConstraints_t::const_iterator it1
+ (constraints_.begin ()); it1 != constraints_.end ();
+ ++it1) {
+ for (ConstraintsAndComplements_t::const_iterator it2
+ (cac.begin ()); it2 != cac.end (); ++it2) {
+ if (((**it1 == *(it2->complement)) &&
+ (**it == *(it2->both))) ||
+ ((**it1 == *(it2->both)) &&
+ (**it == *(it2->complement)))) {
+ assert (sameRightHandSide_.count (*it1) == 0);
+ assert (sameRightHandSide_.count (*it) == 0);
+ sameRightHandSide_ [*it1] = *it;
+ sameRightHandSide_ [*it] = *it1;
+ }
+ }
+ }
+ constraints_.push_back (*it);
+ hppDout (info, "Adding constraint \"" << name << "\"");
+ hppDout (info, "Edge \"" << edge->name () << "\"");
+ hppDout (info, "parameter size: " << (*it)->parameterSize ());
+
+ }
+ }
+ }
+ }
+ }
+ }
+
+ bool StatesPathFinder::findTransitions (GraphSearchData& d) const
+ {
+ while (! d.queue1.empty())
+ {
+ GraphSearchData::state_with_depth_ptr_t _state = d.queue1.front();
+
+ const GraphSearchData::state_with_depth& parent = d.getParent(_state);
+ if (parent.l >= d.maxDepth) return true;
+ d.queue1.pop();
+
+ bool done = false;
+
+ const Neighbors_t& neighbors = _state.state->first->neighbors();
+ for (Neighbors_t::const_iterator _n = neighbors.begin();
+ _n != neighbors.end(); ++_n) {
+ EdgePtr_t transition = _n->second;
+
+ // Don't even consider level set edges
+ if (containsLevelSet(transition)) continue;
+
+ // Avoid identical consecutive transition
+ if (transition == parent.e) continue;
+
+ // Insert parent
+ d.queue1.push (
+ d.addParent (_state, transition)
+ );
+
+ done = done || (transition->stateTo() == d.s2);
+ }
+ if (done) break;
+ }
+ return false;
+ }
+
+ Edges_t StatesPathFinder::getTransitionList (
+ 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);
+ Edges_t transitions;
+ if (i >= roots.size()) return transitions;
+
+ const GraphSearchData::state_with_depth* current = &roots[i];
+ transitions.reserve (current->l);
+ graph::WaypointEdgePtr_t we;
+ while (current->e) {
+ assert (current->l > 0);
+ we = HPP_DYNAMIC_PTR_CAST(graph::WaypointEdge, current->e);
+ if (we) {
+ for (int i = (int)we->nbWaypoints(); i >= 0; --i)
+ transitions.push_back(we->waypoint(i));
+ } else {
+ transitions.push_back(current->e);
+ }
+ current = &d.parent1.at(current->s)[current->i];
+ }
+ std::reverse (transitions.begin(), transitions.end());
+ return transitions;
+ }
+
+ struct StatesPathFinder::OptimizationData
+ {
+ typedef constraints::solver::HierarchicalIterative::Saturation_t
+ Saturation_t;
+ enum RightHandSideStatus_t {
+ // Constraint is not in solver for this waypoint
+ ABSENT,
+ // right hand side of constraint for this waypoint is equal to
+ // right hand side for previous waypoint
+ EQUAL_TO_PREVIOUS,
+ // right hand side of constraint for this waypoint is equal to
+ // right hand side for initial configuration
+ EQUAL_TO_INIT,
+ // right hand side of constraint for this waypoint is equal to
+ // right hand side for goal configuration
+ EQUAL_TO_GOAL
+ }; // enum RightHandSideStatus_t
+ const std::size_t N, nq, nv;
+ std::vector <Solver_t> solvers;
+ std::vector <bool> isTargetWaypoint;
+ // Waypoints lying in each intermediate state
+ matrix_t waypoint;
+ Configuration_t q1, q2;
+ core::DevicePtr_t robot;
+ // Matrix specifying for each constraint and each waypoint how
+ // the right hand side is initialized in the solver.
+ Eigen::Matrix < LiegroupElement, Eigen::Dynamic, Eigen::Dynamic > M_rhs;
+ Eigen::Matrix < RightHandSideStatus_t, Eigen::Dynamic, Eigen::Dynamic >
+ M_status;
+ // Number of trials to generate each waypoint configuration
+ OptimizationData (const core::ProblemConstPtr_t _problem,
+ const Configuration_t& _q1,
+ const Configuration_t& _q2,
+ const Edges_t& transitions
+ ) :
+ N (transitions.size () - 1), nq (_problem->robot()->configSize()),
+ nv (_problem->robot()->numberDof()),
+ solvers (N, _problem->robot()->configSpace ()),
+ waypoint (nq, N), q1 (_q1), q2 (_q2),
+ robot (_problem->robot()),
+ M_rhs (), M_status ()
+ {
+ waypoint.setZero();
+ for (auto solver: solvers){
+ // Set maximal number of iterations for each solver
+ solver.maxIterations(_problem->getParameter
+ ("StatesPathFinder/maxIteration").intValue());
+ // Set error threshold for each solver
+ solver.errorThreshold(_problem->getParameter
+ ("StatesPathFinder/errorThreshold").floatValue());
+ }
+ assert (transitions.size () > 0);
+ isTargetWaypoint.assign(N+1, false);
+ for (std::size_t i = 0; i < transitions.size(); i++)
+ isTargetWaypoint[i] = transitions[i]->stateTo()->isWaypoint();
+ }
+ };
+
+ bool StatesPathFinder::checkConstantRightHandSide (size_type index)
+ {
+ OptimizationData& d = *optData_;
+ const ImplicitPtr_t c (constraints_ [index]);
+ LiegroupElement rhsInit(c->function().outputSpace());
+ c->rightHandSideFromConfig (d.q1, rhsInit);
+ LiegroupElement rhsGoal(c->function().outputSpace());
+ c->rightHandSideFromConfig (d.q2, rhsGoal);
+ // Check that right hand sides are close to each other
+ value_type eps (problem_->constraintGraph ()->errorThreshold ());
+ value_type eps2 (eps * eps);
+ if ((rhsGoal - rhsInit).squaredNorm () > eps2) {
+ return false;
+ }
+ // Matrix of solver right hand sides
+ for (size_type j=0; j<d.M_rhs.cols (); ++j) {
+ d.M_rhs (index, j) = rhsInit;
+ }
+ return true;
+ }
+
+ bool StatesPathFinder::checkWaypointRightHandSide
+ (std::size_t ictr, std::size_t jslv) const
+ {
+ const OptimizationData& d = *optData_;
+ ImplicitPtr_t c = constraints_ [ictr]->copy();
+ LiegroupElement rhsNow(c->function().outputSpace());
+ assert(rhsNow.size() == c->rightHandSideSize());
+ c->rightHandSideFromConfig (d.waypoint.col (jslv), rhsNow);
+ c = constraints_ [ictr]->copy();
+ LiegroupElement rhsOther(c->function().outputSpace());
+ switch (d.M_status(ictr, jslv)) {
+ case OptimizationData::EQUAL_TO_INIT:
+ c->rightHandSideFromConfig (d.q1, rhsOther);
+ break;
+ case OptimizationData::EQUAL_TO_GOAL:
+ c->rightHandSideFromConfig (d.q2, rhsOther);
+ break;
+ case OptimizationData::EQUAL_TO_PREVIOUS:
+ c->rightHandSideFromConfig (d.waypoint.col (jslv-1), rhsOther);
+ break;
+ case OptimizationData::ABSENT:
+ default:
+ return true;
+ }
+ hpp::pinocchio::vector_t diff = rhsOther - rhsNow;
+ hpp::pinocchio::vector_t diffmask(diff.size());
+ for (auto k: c->activeRows()) // filter with constraint mask
+ for (size_type kk = k.first; kk < k.first + k.second; kk++)
+ diffmask[kk] = diff[kk];
+ value_type eps (problem_->constraintGraph ()->errorThreshold ());
+ value_type eps2 (eps * eps);
+ return diffmask.squaredNorm () < eps2;
+ }
+
+ bool StatesPathFinder::checkWaypointRightHandSide
+ (std::size_t jslv) const
+ {
+ for (std::size_t ictr = 0; ictr < constraints_.size(); ictr++)
+ if (!checkWaypointRightHandSide(ictr, jslv))
+ return false;
+ return true;
+ }
+
+ void StatesPathFinder::displayRhsMatrix ()
+ {
+ OptimizationData& d = *optData_;
+ Eigen::Matrix < LiegroupElement, Eigen::Dynamic, Eigen::Dynamic >& m
+ = d.M_rhs;
+
+ for (std::size_t i = 0; i < constraints_.size(); i++) {
+ const ImplicitPtr_t& constraint = constraints_[i];
+ for (std::size_t j = 0; j < d.solvers.size(); j++) {
+ const vectorIn_t& config = d.waypoint.col(j);
+ LiegroupElement le(constraint->function().outputSpace());
+ constraint->rightHandSideFromConfig(d.waypoint.col(j), le);
+ m(i,j) = le;
+ }
+ }
+
+ std::ostringstream oss; oss.precision (2);
+ oss << "\\documentclass[12pt,landscape]{article}" << std::endl;
+ oss << "\\usepackage[a3paper]{geometry}" << std::endl;
+ oss << "\\begin {document}" << std::endl;
+
+ for (size_type ii = 0; ii < (m.cols()+7)/8; ii++) {
+ size_type j0 = ii*8;
+ size_type j1 = std::min(ii*8+8, m.cols());
+ size_type dj = j1-j0;
+ oss << "\\begin {tabular}{";
+ for (size_type j=0; j<dj + 2; ++j)
+ oss << "c";
+ oss << "}" << std::endl;
+ oss << "Constraint & mask";
+ for (size_type j=j0; j<j1; ++j)
+ oss << " & WP" << j;
+ oss << "\\\\" << std::endl;
+ for (size_type i=0; i<m.rows (); ++i) {
+ std::vector<int> mask(constraints_[i]->parameterSize());
+ for (auto k: constraints_[i]->activeRows())
+ for (size_type kk = k.first; kk < k.first + k.second; kk++)
+ mask[kk] = 1;
+ std::ostringstream oss1; oss1.precision (2);
+ std::ostringstream oss2; oss2.precision (2);
+ oss1 << constraints_ [i]->function ().name () << " & ";
+
+ oss1 << "$\\left(\\begin{array}{c} ";
+ for (std::size_t k=0; k<mask.size (); ++k) {
+ oss1 << mask[k] << "\\\\ ";
+ }
+ oss1 << "\\end{array}\\right)$" << std::endl;
+ oss1 << " & " << std::endl;
+
+ for (size_type j=j0; j<j1; ++j) {
+ if (d.M_status(i,j) != OptimizationData::ABSENT || (j < m.cols () - 1 &&
+ d.M_status(i,j+1) == OptimizationData::EQUAL_TO_PREVIOUS)) {
+ oss2 << "$\\left(\\begin{array}{c} ";
+ for (size_type k=0; k<m (i,j).size (); ++k) {
+ oss2 << ((abs(m (i,j).vector()[k]) < 1e-6) ? 0 : m (i,j).vector()[k]) << "\\\\ ";
+ }
+ oss2 << "\\end{array}\\right)$" << std::endl;
+ }
+ if (j < j1 - 1) {
+ oss2 << " & " << std::endl;
+ }
+ }
+ std::string str2 = oss2.str();
+ if (str2.size() > 50) { // don't display constraints used nowhere
+ oss << oss1.str() << str2 << "\\\\" << std::endl;
+ }
+ }
+ oss << "\\end{tabular}" << std::endl << std::endl;
+ }
+ oss << "\\end{document}" << std::endl;
+
+ std::string s = oss.str ();
+ std::string s2 = "";
+ for (std::size_t i=0; i < s.size(); i++) {
+ if (s[i] == '_') s2 += "\\_";
+ else s2.push_back(s[i]);
+ }
+ hppDout (info, s2);
+ }
+
+ void StatesPathFinder::displayStatusMatrix (const graph::Edges_t& transitions)
+ {
+ const Eigen::Matrix < OptimizationData::RightHandSideStatus_t, Eigen::Dynamic, Eigen::Dynamic >&
+ m = optData_->M_status;
+ std::ostringstream oss; oss.precision (5);
+ oss << "\\documentclass[12pt,landscape]{article}" << std::endl;
+ oss << "\\usepackage[a3paper]{geometry}" << std::endl;
+ oss << "\\begin {document}" << std::endl;
+ oss << "\\paragraph{Edges}" << std::endl;
+ oss << "\\begin{enumerate}" << std::endl;
+ for (auto edge : transitions) {
+ oss << "\\item \\texttt{" << edge->name() << "}" << std::endl;
+ }
+ oss << "\\end{enumerate}" << std::endl;
+ oss << "\\begin {tabular}{l|";
+ for (size_type j=0; j<m.cols (); ++j)
+ if (transitions[j]->stateTo()->isWaypoint()) oss << "c";
+ else oss << "|c|";
+ oss << "|}" << std::endl;
+ oss << "Constraint";
+ 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 << "\\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 << " & ";
+ }
+ oss << "\\\\" << std::endl;
+ }
+ oss << "\\end{tabular}" << std::endl;
+ oss << "\\end{document}" << std::endl;
+
+ std::string s = oss.str ();
+ std::string s2 = "";
+ for (std::size_t i=0; i < s.size(); i++) {
+ if (s[i] == '_') s2 += "\\_";
+ else s2.push_back(s[i]);
+ }
+ hppDout (info, s2);
+ }
+
+ bool StatesPathFinder::contains
+ (const Solver_t& solver, const ImplicitPtr_t& c) const
+ {
+ if (solver.contains (c)) return true;
+ std::map <ImplicitPtr_t, ImplicitPtr_t>::const_iterator it
+ (sameRightHandSide_.find (c));
+ if (it != sameRightHandSide_.end () && solver.contains (it->second))
+ return true;
+ return false;
+ }
+
+ bool StatesPathFinder::buildOptimizationProblem
+ (const graph::Edges_t& transitions)
+ {
+ OptimizationData& d = *optData_;
+ if (d.N == 0) return false;
+ d.M_status.resize (constraints_.size (), d.N);
+ d.M_status.fill (OptimizationData::ABSENT);
+ d.M_rhs.resize (constraints_.size (), d.N);
+ d.M_rhs.fill (LiegroupElement ());
+ size_type index = 0;
+ // Loop over constraints
+ for (NumericalConstraints_t::const_iterator it (constraints_.begin ());
+ it != constraints_.end (); ++it) {
+ const ImplicitPtr_t& c (*it);
+ // Loop forward over waypoints to determine right hand sides equal
+ // to initial configuration
+ for (std::size_t j = 0; j < d.N; ++j) {
+ // Get transition solver
+ const Solver_t& trSolver
+ (transitions [j]->pathConstraint ()->configProjector ()->solver ());
+ if (contains (trSolver, c)) {
+ if ((j==0) || d.M_status (index, j-1) ==
+ OptimizationData::EQUAL_TO_INIT) {
+ d.M_status (index, j) = OptimizationData::EQUAL_TO_INIT;
+ } else {
+ d.M_status (index, j) = OptimizationData::EQUAL_TO_PREVIOUS;
+ }
+ }
+ }
+ // Loop backward over waypoints to determine right hand sides equal
+ // to final configuration
+ for (size_type j = d.N-1; j > 0; --j) {
+ // Get transition solver
+ const Solver_t& trSolver
+ (transitions [(std::size_t)j+1]->pathConstraint ()->
+ configProjector ()->solver ());
+ if (contains (trSolver, c)) {
+ if ((j==(size_type) d.N-1) || d.M_status (index, j+1) ==
+ OptimizationData::EQUAL_TO_GOAL) {
+ // If constraint right hand side is already equal to
+ // initial config, check that right hand side is equal
+ // for init and goal configs.
+ if (d.M_status (index, j) ==
+ OptimizationData::EQUAL_TO_INIT) {
+ if (checkConstantRightHandSide (index)) {
+ // stop for this constraint
+ break;
+ } else {
+ // Right hand side of constraint should be equal along the
+ // whole path but is different at init and at goal configs.
+ return false;
+ }
+ } else {
+ d.M_status (index, j) = OptimizationData::EQUAL_TO_GOAL;
+ }
+ }
+ } else {
+ break;
+ }
+ }
+ ++index;
+ } // for (NumericalConstraints_t::const_iterator it
+ displayStatusMatrix (transitions);
+ // Fill solvers with target constraints of transition
+ for (std::size_t j = 0; j < d.N; ++j) {
+ d.solvers [j] = transitions [j]->
+ targetConstraint ()->configProjector ()->solver ();
+ if (j > 0 && j < d.N-1) {
+ const Solver_t& otherSolver = transitions [j+1]->
+ pathConstraint ()->configProjector ()->solver ();
+ for (std::size_t i = 0; i < constraints_.size (); i++) {
+ if (d.M_status(i, j-1) == OptimizationData::ABSENT &&
+ d.M_status(i, j) == OptimizationData::EQUAL_TO_GOAL &&
+ !contains(d.solvers[j], constraints_[i]) &&
+ otherSolver.contains(constraints_[i])) {
+ d.solvers[j].add(constraints_[i]);
+ hppDout(info, "Adding missing constraint " << constraints_[i]->function().name()
+ << " to solver for waypoint" << j+1);
+ }
+ }
+ }
+ }
+
+ return true;
+ }
+
+ bool StatesPathFinder::checkSolverRightHandSide
+ (std::size_t ictr, std::size_t jslv) const
+ {
+ const OptimizationData& d = *optData_;
+ ImplicitPtr_t c = constraints_ [ictr]->copy();
+ const Solver_t& solver = d.solvers[jslv];
+ vector_t rhs(c->rightHandSideSize());
+ solver.getRightHandSide(c, rhs);
+ LiegroupElement rhsNow(c->function().outputSpace());
+ assert(rhsNow.size() == rhs.size());
+ rhsNow.vector() = rhs;
+ LiegroupElement rhsOther(c->function().outputSpace());
+ switch (d.M_status(ictr, jslv)) {
+ case OptimizationData::EQUAL_TO_INIT:
+ c->rightHandSideFromConfig (d.q1, rhsOther);
+ break;
+ case OptimizationData::EQUAL_TO_GOAL:
+ c->rightHandSideFromConfig (d.q2, rhsOther);
+ break;
+ case OptimizationData::EQUAL_TO_PREVIOUS:
+ c->rightHandSideFromConfig (d.waypoint.col (jslv-1), rhsOther);
+ break;
+ case OptimizationData::ABSENT:
+ default:
+ return true;
+ }
+ hpp::pinocchio::vector_t diff = rhsOther - rhsNow;
+ hpp::pinocchio::vector_t diffmask(diff.size());
+ for (auto k: c->activeRows()) // filter with constraint mask
+ for (size_type kk = k.first; kk < k.first + k.second; kk++)
+ diffmask[kk] = diff[kk];
+ value_type eps (problem_->constraintGraph ()->errorThreshold ());
+ value_type eps2 (eps * eps);
+ if (diffmask.squaredNorm () > eps2) hppDout(warning, diffmask.squaredNorm () << " vs " << eps2);
+ return diffmask.squaredNorm () < eps2;
+ }
+
+ bool StatesPathFinder::checkSolverRightHandSide
+ (std::size_t jslv) const
+ {
+ for (std::size_t ictr = 0; ictr < constraints_.size(); ictr++)
+ if (!checkSolverRightHandSide(ictr, jslv))
+ return false;
+ return true;
+ }
+
+ bool StatesPathFinder::buildOptimizationProblemFromNames(std::vector<std::string> names)
+ {
+ graph::Edges_t transitions;
+ graph::GraphPtr_t cg (problem_->constraintGraph ());
+ for (const std::string& name: names) {
+ for (std::size_t i = 0; i < cg->nbComponents (); ++i) {
+ graph::EdgePtr_t edge (HPP_DYNAMIC_PTR_CAST (graph::Edge, cg->get (i).lock ()));
+ if (edge && edge->name() == name)
+ transitions.push_back(edge);
+ }
+ }
+ return buildOptimizationProblem(transitions);
+ }
+
+ void StatesPathFinder::preInitializeRHS(std::size_t j, Configuration_t& q) {
+ OptimizationData& d = *optData_;
+ Solver_t& solver (d.solvers [j]);
+ for (std::size_t i=0; i<constraints_.size (); ++i) {
+ const ImplicitPtr_t& c (constraints_ [i]);
+ bool ok = true;
+ switch (d.M_status ((size_type)i, (size_type)j)) {
+ case OptimizationData::EQUAL_TO_INIT:
+ ok = solver.rightHandSideFromConfig (c, d.q1);
+ break;
+ case OptimizationData::EQUAL_TO_GOAL:
+ ok = solver.rightHandSideFromConfig (c, d.q2);
+ break;
+ case OptimizationData::EQUAL_TO_PREVIOUS:
+ ok = solver.rightHandSideFromConfig (c, q);
+ break;
+ case OptimizationData::ABSENT:
+ default:
+ ;
+ }
+ ok |= contains(solver, constraints_[i]);
+ if(!ok) {
+ std::ostringstream err_msg;
+ err_msg << "\nConstraint " << i << " missing for waypoint " << j+1
+ << " (" << c->function().name() << ")\n"
+ << "The constraints in this solver are:\n";
+ for (const std::string& name: constraintNamesFromSolverAtWaypoint(j+1))
+ err_msg << name << "\n";
+ hppDout(warning, err_msg.str());
+ }
+ assert(ok);
+ }
+ }
+
+ bool StatesPathFinder::analyseOptimizationProblem
+ (const graph::Edges_t& transitions)
+ {
+ OptimizationData& d = *optData_;
+ size_type nTriesMax = problem_->getParameter
+ ("StatesPathFinder/maxTriesCollisionAnalysis").intValue();
+ if (nTriesMax == 0) return true;
+ for (std::size_t wp = 1; wp <= d.solvers.size(); wp++) {
+ std::size_t j = wp-1;
+ const Solver_t& solver = d.solvers[j];
+ using namespace core;
+ Solver_t::Status status;
+ size_type tries = 0;
+ Configuration_t q;
+ do {
+ q = *(problem()->configurationShooter()->shoot());
+ preInitializeRHS(j, q);
+ status = solver.solve (q,
+ constraints::solver::lineSearch::Backtracking ());
+ } while ((status != Solver_t::SUCCESS) && (++tries <= nTriesMax));
+ if (tries > nTriesMax) {
+ hppDout(info, "Collision analysis stopped at WP " << wp
+ << " because of too many bad solve statuses");
+ return false;
+ }
+ CollisionValidationPtr_t collisionValidations = CollisionValidation::create(problem_->robot());
+ collisionValidations->checkParameterized(true);
+ collisionValidations->computeAllContacts(true);
+ ValidationReportPtr_t validationReport;
+ bool ok = true;
+ if (!collisionValidations->validate (q, validationReport)) {
+ AllCollisionsValidationReportPtr_t allReports = HPP_DYNAMIC_PTR_CAST(
+ AllCollisionsValidationReport, validationReport);
+ assert(allReports);
+ std::size_t nbReports = allReports->collisionReports.size();
+ hppDout(info, wp << " nbReports: " << nbReports);
+ for (std::size_t i = 0; i < nbReports; i++) {
+ CollisionValidationReportPtr_t& report = allReports->collisionReports[i];
+ JointConstPtr_t j1 = report->object1->joint();
+ JointConstPtr_t j2 = report->object2->joint();
+ if (!j1 || !j2) continue;
+ const EdgePtr_t& edge = transitions[wp];
+ RelativeMotion::matrix_type m = edge->relativeMotion();
+ RelativeMotion::RelativeMotionType rmt =
+ m(RelativeMotion::idx(j1), RelativeMotion::idx(j2));
+ hppDout(info, "report " << i << " joints names \n" <<
+ j1->name() << "\n" << j2->name() << "\n" << rmt);
+ if (rmt == RelativeMotion::RelativeMotionType::Unconstrained)
+ continue;
+ ok = false;
+ break;
+ }
+ }
+ if (!ok) {
+ hppDout(info, "Analysis found a collision at WP " << wp);
+ return false;
+ }
+ hppDout(info, "Analysis at WP " << wp << " passed after " << tries << " solve tries");
+ }
+ return true;
+ }
+
+ void StatesPathFinder::initializeRHS(std::size_t j) {
+ OptimizationData& d = *optData_;
+ Solver_t& solver (d.solvers [j]);
+ for (std::size_t i=0; i<constraints_.size (); ++i) {
+ const ImplicitPtr_t& c (constraints_ [i]);
+ bool ok = true;
+ switch (d.M_status ((size_type)i, (size_type)j)) {
+ case OptimizationData::EQUAL_TO_PREVIOUS:
+ assert (j != 0);
+ ok = solver.rightHandSideFromConfig (c, d.waypoint.col (j-1));
+ break;
+ case OptimizationData::EQUAL_TO_INIT:
+ ok = solver.rightHandSideFromConfig (c, d.q1);
+ break;
+ case OptimizationData::EQUAL_TO_GOAL:
+ ok = solver.rightHandSideFromConfig (c, d.q2);
+ break;
+ case OptimizationData::ABSENT:
+ default:
+ ;
+ }
+ ok |= contains(solver, constraints_[i]);
+ if(!ok) {
+ std::ostringstream err_msg;
+ err_msg << "\nConstraint " << i << " missing for waypoint " << j+1
+ << " (" << c->function().name() << ")\n"
+ << "The constraints in this solver are:\n";
+ for (const std::string& name: constraintNamesFromSolverAtWaypoint(j+1))
+ err_msg << name << "\n";
+ hppDout(warning, err_msg.str());
+ }
+ assert(ok);
+ }
+ }
+
+ void StatesPathFinder::initWPRandom(std::size_t wp) {
+ assert(wp >=1 && wp <= (std::size_t) optData_->waypoint.cols());
+ initializeRHS(wp-1);
+ optData_->waypoint.col (wp-1) = *(problem()->configurationShooter()->shoot());
+ }
+ void StatesPathFinder::initWPNear(std::size_t wp) {
+ assert(wp >=1 && wp <= (std::size_t) optData_->waypoint.cols());
+ initializeRHS(wp-1);
+ if (wp == 1)
+ optData_->waypoint.col (wp-1) = optData_->q1;
+ else
+ optData_->waypoint.col (wp-1) = optData_->waypoint.col (wp-2);
+ }
+ void StatesPathFinder::initWP(std::size_t wp, ConfigurationIn_t q) {
+ assert(wp >=1 && wp <= (std::size_t) optData_->waypoint.cols());
+ initializeRHS(wp-1);
+ optData_->waypoint.col (wp-1) = q;
+ }
+
+ int StatesPathFinder::solveStep(std::size_t wp) {
+ assert(wp >=1 && wp <= (std::size_t) optData_->waypoint.cols());
+ std::size_t j = wp-1;
+ Solver_t& solver (optData_->solvers [j]);
+ Solver_t::Status status = solver.solve (optData_->waypoint.col (j),
+ constraints::solver::lineSearch::Backtracking ());
+ if (status == Solver_t::SUCCESS) {
+ assert (checkWaypointRightHandSide(j));
+ core::ConfigValidationsPtr_t configValidations = problem_->configValidations();
+ core::ConfigValidationsPtr_t configValidations2 = core::ConfigValidations::create();
+ core::CollisionValidationPtr_t colValidation = core::CollisionValidation::create(problem()->robot());
+ const graph::Edges_t& edges = lastBuiltTransitions_;
+ matrix_t secmat1 = edges[j]->securityMargins();
+ matrix_t secmat2 = edges[j+1]->securityMargins();
+ matrix_t maxmat = secmat1.cwiseMax(secmat2);
+ colValidation->setSecurityMargins(maxmat);
+ configValidations2->add(colValidation);
+ core::ValidationReportPtr_t report;
+ if (!configValidations->validate (optData_->waypoint.col (j), report))
+ return 2;
+ if (!configValidations2->validate (optData_->waypoint.col (j), report)) {
+ //hppDout(warning, maxmat);
+ //hppDout(warning, pinocchio::displayConfig(optData_->waypoint.col (j)));
+ //hppDout(warning, *report);
+ //return 4;
+ return 3;
+ }
+ return 0;
+ }
+ return 1;
+ }
+
+ std::string StatesPathFinder::displayConfigsSolved() const {
+ const OptimizationData& d = *optData_;
+ std::ostringstream oss;
+ oss << "configs = [" << std::endl;
+ oss << " " << pinocchio::displayConfig(d.q1) << ", # 0" << std::endl;
+ for (size_type j = 0; j < d.waypoint.cols(); j++)
+ oss << " " << pinocchio::displayConfig(d.waypoint.col(j)) << ", # " << j+1 << std::endl;
+ oss << " " << pinocchio::displayConfig(d.q2)
+ << " # " << d.waypoint.cols()+1 << std::endl << "]" << std::endl;
+ std::string ans = oss.str();
+ hppDout(info, ans);
+ return ans;
+ }
+
+ Configuration_t StatesPathFinder::configSolved (std::size_t wp) const {
+ const OptimizationData& d = *optData_;
+ std::size_t nbs = optData_->solvers.size();
+ if (wp == 0)
+ return d.q1;
+ if (wp >= nbs+1)
+ return d.q2;
+ return d.waypoint.col(wp-1);
+ }
+
+ bool StatesPathFinder::solveOptimizationProblem ()
+ {
+ OptimizationData& d = *optData_;
+ // Try to solve with sets of random configs for each waypoint
+ std::size_t nTriesMax = problem_->getParameter
+ ("StatesPathFinder/nTriesUntilBacktrack").intValue();
+ std::size_t nTriesMax1 = nTriesMax*10; // more tries for the first waypoint
+ std::size_t nFailsMax = nTriesMax*20; // fails before reseting the whole solution
+ std::vector<std::size_t> nTriesDone(d.solvers.size()+1, 0);
+ std::size_t nFails = 0;
+ std::size_t wp = 1; // waypoint index starting at 1 (wp 0 = q1)
+
+ while (wp <= d.solvers.size()) {
+ // enough tries for a waypoint: backtrack or stop
+ while (nTriesDone[wp] >= nTriesMax) {
+ if (wp == 1) {
+ if (nTriesDone[wp] < nTriesMax1)
+ break;
+ displayConfigsSolved();
+ return false; // too many tries that need to reset the entire solution
+ }
+ do {
+ nTriesDone[wp] = 0;
+ wp--; // backtrack: try a new solution for the latest waypoint
+ } while (wp>1 && d.isTargetWaypoint[wp-1]);
+ }
+
+ // Completely reset a solution when too many tries have failed
+ if (wp > 1 && nFails >= nFailsMax) {
+ for (std::size_t k = 2; k <= d.solvers.size(); k++)
+ nTriesDone[k] = 0;
+ wp = 1;
+ if (nTriesDone[1] >= nTriesMax1) {
+ displayConfigsSolved();
+ return false;
+ }
+ }
+ // Reset the fail counter while the solution is empty
+ if (wp == 1)
+ nFails = 0;
+
+ // Initialize right hand sides, and
+ // Choose a starting configuration for the solver.solve method:
+ // - from previous waypoint if it's the first time we see this solver
+ // given current solvers 0 to j-1
+ // - with a random configuration if the other initialization has been
+ // tried and failed
+ if (nTriesDone[wp] == 0)
+ initWPNear(wp);
+ else
+ initWPRandom(wp);
+
+ nTriesDone[wp]++; // Backtrack to last state when this gets too big
+
+ int out = solveStep(wp);
+ hppDout(info, "solveStep exit code at WP" << wp << ": " << out);
+ switch (out) {
+ case 0: // Valid solution, go to next waypoint
+ wp++; break;
+ case 1: // Collision. If that happens too much, go back to first waypoint
+ nFails++; break;
+ case 2: // Bad solve status, considered usual so nothing more
+ case 3:
+ break;
+ default:
+ throw(std::logic_error("Unintended exit code for solveStep"));
+ }
+ }
+
+ displayConfigsSolved();
+ displayRhsMatrix ();
+
+ return true;
+ }
+
+ core::Configurations_t StatesPathFinder::buildConfigList () const
+ {
+ OptimizationData& d = *optData_;
+ core::Configurations_t pv;
+ ConfigurationPtr_t q1 (new Configuration_t (d.q1));
+ pv.push_back(q1);
+ for (std::size_t i = 0; i < d.N; ++i) {
+ ConfigurationPtr_t q (new Configuration_t (d.waypoint.col (i)));
+ pv.push_back(q);
+ }
+ ConfigurationPtr_t q2 (new Configuration_t (d.q2));
+ pv.push_back(q2);
+ return pv;
+ }
+
+ core::Configurations_t StatesPathFinder::computeConfigList (
+ ConfigurationIn_t q1, ConfigurationIn_t q2)
+ {
+ const graph::GraphPtr_t& graph(problem_->constraintGraph ());
+ GraphSearchData d;
+ d.s1 = graph->getState (q1);
+ d.s2 = graph->getState (q2);
+ d.maxDepth = problem_->getParameter
+ ("StatesPathFinder/maxDepth").intValue();
+
+ // Find
+ d.queue1.push (d.addInitState());
+ std::size_t idxSol = (d.s1 == d.s2 ? 1 : 0);
+ if (idxSol_ < idxSol) idxSol_ = idxSol;
+
+ bool maxDepthReached;
+ while (!(maxDepthReached = findTransitions (d))) { // mut
+ Edges_t transitions = getTransitionList (d, idxSol); // const, const
+ while (! transitions.empty()) {
+ if (idxSol >= idxSol_) {
+#ifdef HPP_DEBUG
+ std::ostringstream ss;
+ ss << " Trying solution " << idxSol << ": \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
+ if (optData_) {
+ delete optData_;
+ optData_ = nullptr;
+ }
+ optData_ = new OptimizationData (problem(), q1, q2, transitions);
+
+ if (buildOptimizationProblem (transitions)) {
+ lastBuiltTransitions_ = transitions;
+ if (skipColAnalysis_ || analyseOptimizationProblem (transitions)) {
+ if (solveOptimizationProblem ()) {
+ core::Configurations_t path = buildConfigList ();
+ hppDout (warning, " Solution " << idxSol << ": solved configurations list");
+ return path;
+ } else {
+ hppDout (info, " Failed solution " << idxSol << " at step 5 (solve opt pb)");
+ }
+ } else {
+ hppDout (info, " Failed solution " << idxSol << " at step 4 (analyse opt pb)");
+ }
+ } else {
+ hppDout (info, " Failed solution " << idxSol << " at step 3 (build opt pb)");
+ }
+ } // if (idxSol >= idxSol_)
+ transitions = getTransitionList(d, ++idxSol);
+ if (idxSol_ < idxSol) idxSol_ = idxSol;
+ }
+ }
+ core::Configurations_t empty_path;
+ ConfigurationPtr_t q (new Configuration_t (q1));
+ empty_path.push_back(q);
+ return empty_path;
+ }
+
+ core::PathVectorPtr_t StatesPathFinder::pathFromConfigList (std::size_t i) const
+ {
+ hppDout(info, "calling StatesPathFinder::pathFromConfigList for transition " << i);
+ try {
+ const Edges_t& transitions = lastBuiltTransitions_;
+ InStatePathPtr_t planner = InStatePath::create(problem_);
+ planner->addOptimizerType("Graph-RandomShortcut");
+ planner->plannerType("DiffusingPlanner");
+ planner->maxIterPlanner(problem_->getParameter
+ ("StatesPathFinder/innerPlannerMaxIterations").intValue());
+ planner->timeOutPlanner(problem_->getParameter
+ ("StatesPathFinder/innerPlannerTimeOut").floatValue());
+ planner->resetRoadmap(true);
+ ConfigurationPtr_t q1 (new Configuration_t (configSolved(i)));
+ ConfigurationPtr_t q2 (new Configuration_t (configSolved(i+1)));
+ hppDout(info, 3);
+ planner->setEdge(transitions[i]);
+ hppDout(info, 4);
+ planner->setInit(q1);
+ planner->setGoal(q2);
+ hppDout(info, "calling InStatePath::computePath for transition " << i);
+ return planner->solve();
+ } catch(const std::exception&(e)) {
+ std::ostringstream oss;
+ oss << "Error " << e.what() << "\n";
+ oss << "Solution \"latest\":\nFailed to build path at edge " << i << ": ";
+ oss << lastBuiltTransitions_[i]->pathConstraint()->name();
+ hppDout(warning, oss.str());
+ core::PathVectorPtr_t empty;
+ return empty;
+ }
+ }
+
+ void StatesPathFinder::reset() {
+ idxSol_ = 0;
+ if (optData_) {
+ delete optData_;
+ optData_ = nullptr;
+ }
+ lastBuiltTransitions_.clear();
+ idxConfigList_ = 0;
+ nTryConfigList_ = 0;
+ }
+
+ core::PathVectorPtr_t StatesPathFinder::buildPath (ConfigurationIn_t q1, ConfigurationIn_t q2)
+ {
+ reset();
+ size_type nTry = 0;
+ size_type nTryMax = problem_->getParameter
+ ("StatesPathFinder/maxTriesBuildPath").intValue();
+
+ InStatePathPtr_t planner = InStatePath::create(problem_);
+ planner->addOptimizerType("Graph-RandomShortcut");
+ planner->plannerType("DiffusingPlanner");
+ planner->maxIterPlanner(problem_->getParameter
+ ("StatesPathFinder/innerPlannerMaxIterations").intValue());
+ planner->timeOutPlanner(problem_->getParameter
+ ("StatesPathFinder/innerPlannerTimeOut").floatValue());
+ planner->resetRoadmap(true);
+
+ while (true) {
+ skipColAnalysis_ = (nTryConfigList_ >= 1); // already passed, don't redo it
+ core::Configurations_t configList = computeConfigList(q1, q2);
+ if (configList.size() <= 1) {// max depth reached
+ reset();
+ continue;
+ }
+ const Edges_t& transitions = lastBuiltTransitions_;
+ std::size_t i = 0;
+ try {
+ core::PathVectorPtr_t ans = core::PathVector::create (
+ problem()->robot()->configSize(), problem()->robot()->numberDof());
+ for (i = 0; i < configList.size()-1; i++) {
+ ConfigurationPtr_t q1 (new Configuration_t (configSolved(i)));
+ ConfigurationPtr_t q2 (new Configuration_t (configSolved(i+1)));
+ planner->setEdge(transitions[i]);
+ planner->setInit(q1);
+ planner->setGoal(q2);
+ hppDout(info, "calling InStatePath::solve for transition " << i);
+ ans->concatenate(planner->solve());
+ }
+ hppDout(warning, "Solution " << idxSol_ << ": Success"
+ << "\n-----------------------------------------------");
+ return ans;
+ } catch(const std::exception&(e)) {
+ std::ostringstream oss;
+ oss << "Error " << e.what() << "\n";
+ oss << "Solution " << idxSol_ << ": Failed to build path at edge " << i << ": ";
+ oss << lastBuiltTransitions_[i]->name();
+ hppDout(warning, oss.str());
+
+ // Retry nTryMax times to build another solution for the same states list
+ if (++nTry >= nTryMax) {
+ nTry = 0;
+ idxSol_++;
+ }
+ }
+ }
+ core::PathVectorPtr_t empty;
+ return empty;
+ }
+
+ void StatesPathFinder::startSolve ()
+ {
+ assert(problem_);
+ q1_ = problem_->initConfig();
+ assert(q1_);
+ core::Configurations_t q2s = problem_->goalConfigs();
+ assert(q2s.size());
+ q2_ = q2s[0];
+
+ reset();
+
+ planner_ = InStatePath::create(problem_);
+ planner_->addOptimizerType("Graph-RandomShortcut");
+ planner_->plannerType("DiffusingPlanner");
+ planner_->maxIterPlanner(problem_->getParameter
+ ("StatesPathFinder/innerPlannerMaxIterations").intValue());
+ planner_->timeOutPlanner(problem_->getParameter
+ ("StatesPathFinder/innerPlannerTimeOut").floatValue());
+ planner_->resetRoadmap(true);
+ }
+
+ void StatesPathFinder::oneStep ()
+ {
+ if (idxConfigList_ == 0) {
+ solution_ = core::PathVector::create (
+ problem()->robot()->configSize(), problem()->robot()->numberDof());
+ skipColAnalysis_ = (nTryConfigList_ >= 1); // already passed, don't redo it
+ configList_ = computeConfigList(*q1_, *q2_);
+ roadmap()->clear();
+ if (configList_.size() <= 1) { // max depth reached
+ reset();
+ return;
+ }
+ }
+
+ try {
+ const Edges_t& transitions = lastBuiltTransitions_;
+ ConfigurationPtr_t q1 (new Configuration_t (configSolved(idxConfigList_)));
+ ConfigurationPtr_t q2 (new Configuration_t (configSolved(idxConfigList_+1)));
+ planner_->setEdge(transitions[idxConfigList_]);
+ planner_->setInit(q1);
+ planner_->setGoal(q2);
+
+ hppDout(info, "calling InStatePath::solve for transition " << idxConfigList_);
+
+ core::PathVectorPtr_t aux = planner_->solve();
+ for (std::size_t r = 0; r < aux->numberPaths()-1; r++)
+ assert(aux->pathAtRank(r)->end() == aux->pathAtRank(r+1)->initial());
+ solution_->concatenate(aux);
+ planner_->mergeLeafRoadmapWith(roadmap());
+
+ idxConfigList_++;
+ if (idxConfigList_ == configList_.size()-1) {
+ hppDout(warning, "Solution " << idxSol_ << ": Success"
+ << "\n-----------------------------------------------");
+ //solution_ = aux;
+ solved_ = true;
+ }
+
+ } catch(const std::exception&(e)) {
+ std::ostringstream oss;
+ oss << "Error " << e.what() << "\n";
+ oss << "Solution " << idxSol_ << ": Failed to build path at edge " << idxConfigList_ << ": ";
+ oss << lastBuiltTransitions_[idxConfigList_]->name();
+ hppDout(warning, oss.str());
+
+ idxConfigList_ = 0;
+ // Retry nTryMax times to build another solution for the same states list
+ size_type nTryMax = problem_->getParameter
+ ("StatesPathFinder/maxTriesBuildPath").intValue();
+ if (++nTryConfigList_ >= nTryMax) {
+ nTryConfigList_ = 0;
+ idxSol_++;
+ }
+ }
+ }
+
+ core::PathVectorPtr_t StatesPathFinder::solve ()
+ {
+ namespace bpt = boost::posix_time;
+
+ interrupt_ = false;
+ solved_ = false;
+ unsigned long int nIter (0);
+ startSolve ();
+ if (interrupt_) throw std::runtime_error ("Interruption");
+ while (!solved_) {
+ // Execute one step
+ hppStartBenchmark(ONE_STEP);
+ oneStep ();
+ hppStopBenchmark(ONE_STEP);
+ hppDisplayBenchmark(ONE_STEP);
+
+ ++nIter;
+ //solved = problem()->target()->reached (roadmap());
+ if (interrupt_) throw std::runtime_error ("Interruption");
+ }
+ return solution_;
+ }
+
+ std::vector<std::string> StatesPathFinder::constraintNamesFromSolverAtWaypoint
+ (std::size_t wp)
+ {
+ assert (wp > 0 && wp <= optData_->solvers.size());
+ constraints::solver::BySubstitution& solver (optData_->solvers [wp-1]);
+ std::vector<std::string> ans;
+ for (std::size_t i = 0; i < solver.constraints().size(); i++)
+ ans.push_back(solver.constraints()[i]->function().name());
+ return ans;
+ }
+
+ std::vector<std::string> StatesPathFinder::lastBuiltTransitions() const
+ {
+ std::vector<std::string> ans;
+ for (const EdgePtr_t& edge: lastBuiltTransitions_)
+ ans.push_back(edge->name());
+ return ans;
+ }
+
+ using core::Parameter;
+ using core::ParameterDescription;
+
+ HPP_START_PARAMETER_DECLARATION(StatesPathFinder)
+ core::Problem::declareParameter(ParameterDescription(Parameter::INT,
+ "StatesPathFinder/maxDepth",
+ "Maximum number of transitions to look for.",
+ Parameter((size_type)std::numeric_limits<unsigned long int>::infinity())));
+ core::Problem::declareParameter(ParameterDescription(Parameter::INT,
+ "StatesPathFinder/maxIteration",
+ "Maximum number of iterations of the Newton Raphson algorithm.",
+ Parameter((size_type)60)));
+ core::Problem::declareParameter(ParameterDescription(Parameter::FLOAT,
+ "StatesPathFinder/errorThreshold",
+ "Error threshold of the Newton Raphson algorithm.",
+ Parameter(1e-4)));
+ core::Problem::declareParameter(ParameterDescription(Parameter::INT,
+ "StatesPathFinder/nTriesUntilBacktrack",
+ "Number of tries when sampling configurations before backtracking"
+ "in function solveOptimizationProblem.",
+ Parameter((size_type)3)));
+ core::Problem::declareParameter(ParameterDescription(Parameter::INT,
+ "StatesPathFinder/maxTriesCollisionAnalysis",
+ "Number of solve tries before stopping the collision analysis,"
+ "before the actual solving part."
+ "Set to 0 to skip this part of the algorithm.",
+ Parameter((size_type)100)));
+ core::Problem::declareParameter(ParameterDescription(Parameter::INT,
+ "StatesPathFinder/maxTriesBuildPath",
+ "Number of solutions with a given states list to try to build a"
+ "continuous path from, before skipping to the next states list",
+ Parameter((size_type)5)));
+ core::Problem::declareParameter(ParameterDescription(Parameter::FLOAT,
+ "StatesPathFinder/innerPlannerTimeOut",
+ "This will set ::timeOut accordingly in the inner"
+ "planner used for building a path after intermediate"
+ "configurations have been found",
+ Parameter(2.0)));
+ core::Problem::declareParameter(ParameterDescription(Parameter::INT,
+ "StatesPathFinder/innerPlannerMaxIterations",
+ "This will set ::maxIterations accordingly in the inner"
+ "planner used for building a path after intermediate"
+ "configurations have been found",
+ Parameter((size_type)1000)));
+ HPP_END_PARAMETER_DECLARATION(StatesPathFinder)
+ } // namespace pathPlanner
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 3f0ebbef5f51e300a6817e46bc27feafe0a742e5..f923dc2b9842e8a993608438267a57c1e35f5367 100644
--- a/src/problem-solver.cc
+++ b/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",
diff --git a/src/steering-method/cross-state-optimization.cc b/src/steering-method/cross-state-optimization.cc
index 9eff42a7bddfc48a920e4a8e96923c525b4f4def..004afa38cf56bb5315325af28b523d54ab8575ee 100644
--- a/src/steering-method/cross-state-optimization.cc
+++ b/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