hpp-manipulation  5.0.0
Classes for manipulation planning.
states-path-finder.hh
Go to the documentation of this file.
1 // Copyright (c) 2017, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr),
3 // Florent Lamiraux (florent.lamiraux@laas.fr)
4 // Alexandre Thiault (athiault@laas.fr)
5 // Le Quang Anh (quang-anh.le@laas.fr)
6 //
7 // This file is part of hpp-manipulation.
8 // hpp-manipulation is free software: you can redistribute it
9 // and/or modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation, either version
11 // 3 of the License, or (at your option) any later version.
12 //
13 // hpp-manipulation is distributed in the hope that it will be
14 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
15 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // General Lesser Public License for more details. You should have
17 // received a copy of the GNU Lesser General Public License along with
18 // hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
19 
20 #ifndef HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH
21 #define HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH
22 
23 #include <hpp/core/config-projector.hh>
24 #include <hpp/core/config-validations.hh>
25 #include <hpp/core/fwd.hh>
26 #include <hpp/core/path-planner.hh>
27 #include <hpp/core/path.hh>
28 #include <hpp/core/projection-error.hh>
29 #include <hpp/core/validation-report.hh>
31 #include <hpp/manipulation/fwd.hh>
33 
34 namespace hpp {
35 namespace manipulation {
36 namespace pathPlanner {
37 
40 
99 class HPP_MANIPULATION_DLLAPI StatesPathFinder : public core::PathPlanner {
100  public:
101  struct OptimizationData;
102 
103  virtual ~StatesPathFinder(){};
104 
106 
108  const core::ProblemConstPtr_t& problem,
109  const core::RoadmapPtr_t& roadmap);
110 
112 
119  core::Configurations_t computeConfigList(ConfigurationIn_t q1,
120  ConfigurationIn_t q2);
121 
122  // access functions for Python interface
123  std::vector<std::string> constraintNamesFromSolverAtWaypoint(std::size_t wp);
124  std::vector<std::string> lastBuiltTransitions() const;
125  std::string displayConfigsSolved() const;
126  bool buildOptimizationProblemFromNames(std::vector<std::string> names);
127 
128  // Substeps of method solveOptimizationProblem
129  void initWPRandom(std::size_t wp);
130  void initWPNear(std::size_t wp);
131  void initWP(std::size_t wp, ConfigurationIn_t q);
142  };
143 
144  SolveStepStatus solveStep(std::size_t wp);
145 
149  void reset();
150 
151  virtual void startSolve();
152  virtual void oneStep();
155  virtual void tryConnectInitAndGoals();
156 
157  protected:
159  const core::RoadmapPtr_t&);
161 
162  void init(StatesPathFinderWkPtr_t weak) { weak_ = weak; }
163 
164  private:
165  typedef constraints::solver::BySubstitution Solver_t;
166  struct GraphSearchData;
167 
169  void gatherGraphConstraints();
170 
173  bool findTransitions(GraphSearchData& data) const;
174 
176  graph::Edges_t getTransitionList(const GraphSearchData& data,
177  const std::size_t& i) const;
178 
180 
181  // check that the solver either contains exactly same constraint
182  // or a constraint with similar parametrizable form
183  // constraint/both and constraint/complement
184  bool contains(const Solver_t& solver, const ImplicitPtr_t& c) const;
185 
186  // check that the solver either contains exactly same constraint
187  // or a stricter version of the constraint
188  // constraint/both stricter than constraint and stricter than
189  // constraint/complement
190  bool containsStricter(const Solver_t& solver, const ImplicitPtr_t& c) const;
191  bool checkConstantRightHandSide(size_type index);
192  bool buildOptimizationProblem(const graph::Edges_t& transitions);
193 
195  void preInitializeRHS(std::size_t j, Configuration_t& q);
196  bool analyseOptimizationProblem(const graph::Edges_t& transitions);
197  bool analyseOptimizationProblem2(const graph::Edges_t& transitions,
198  core::ProblemConstPtr_t _problem);
199 
201  void initializeRHS(std::size_t j);
202  bool solveOptimizationProblem();
203 
205  core::Configurations_t getConfigList() const;
206 
208  bool checkWaypointRightHandSide(std::size_t ictr, std::size_t jslv) const;
209  bool checkSolverRightHandSide(std::size_t ictr, std::size_t jslv) const;
210  bool checkWaypointRightHandSide(std::size_t jslv) const;
211  bool checkSolverRightHandSide(std::size_t jslv) const;
212 
213  void displayRhsMatrix();
214  void displayStatusMatrix(const graph::Edges_t& transitions);
215 
217  ProblemConstPtr_t problem_;
219  core::ProblemPtr_t inStateProblem_;
220 
222  NumericalConstraints_t constraints_;
224  std::map<std::string, std::size_t> index_;
225 
228  std::map<ImplicitPtr_t, ImplicitPtr_t> sameRightHandSide_;
229 
233  std::map<ImplicitPtr_t, ImplicitPtr_t> stricterConstraints_;
234 
235  mutable OptimizationData* optData_;
236  mutable std::shared_ptr<GraphSearchData> graphData_;
237  graph::Edges_t lastBuiltTransitions_;
238 
246  NumericalConstraints_t goalConstraints_;
247  bool goalDefinedByConstraints_;
248  // Variables used across several calls to oneStep
249  Configuration_t q1_, q2_;
250  core::Configurations_t configList_;
251  std::size_t idxConfigList_;
252  size_type nTryConfigList_;
253  bool solved_, interrupt_;
254 
256  StatesPathFinderWkPtr_t weak_;
257 
258 }; // class StatesPathFinder
260 
261 } // namespace pathPlanner
262 } // namespace manipulation
263 } // namespace hpp
264 
265 #endif // HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH
Definition: states-path-finder.hh:99
std::vector< std::string > constraintNamesFromSolverAtWaypoint(std::size_t wp)
static StatesPathFinderPtr_t createWithRoadmap(const core::ProblemConstPtr_t &problem, const core::RoadmapPtr_t &roadmap)
SolveStepStatus solveStep(std::size_t wp)
StatesPathFinder(const StatesPathFinder &other)
virtual ~StatesPathFinder()
Definition: states-path-finder.hh:103
core::Configurations_t computeConfigList(ConfigurationIn_t q1, ConfigurationIn_t q2)
StatesPathFinder(const core::ProblemConstPtr_t &problem, const core::RoadmapPtr_t &)
SolveStepStatus
Status of the step to solve for a particular waypoint.
Definition: states-path-finder.hh:133
@ VALID_SOLUTION
Valid solution (no collision)
Definition: states-path-finder.hh:135
@ NO_SOLUTION
Bad solve status, no solution from the solver.
Definition: states-path-finder.hh:137
@ COLLISION_AFTER
Solution has collision in edge going from the waypoint.
Definition: states-path-finder.hh:141
@ COLLISION_BEFORE
Solution has collision in edge leading to the waypoint.
Definition: states-path-finder.hh:139
static StatesPathFinderPtr_t create(const core::ProblemConstPtr_t &problem)
std::vector< std::string > lastBuiltTransitions() const
void initWP(std::size_t wp, ConfigurationIn_t q)
bool buildOptimizationProblemFromNames(std::vector< std::string > names)
void init(StatesPathFinderWkPtr_t weak)
Definition: states-path-finder.hh:162
#define HPP_MANIPULATION_DLLAPI
Definition: config.hh:88
std::vector< EdgePtr_t > Edges_t
Definition: fwd.hh:57
shared_ptr< StatesPathFinder > StatesPathFinderPtr_t
Definition: fwd.hh:101
shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:65
shared_ptr< Roadmap > RoadmapPtr_t
Definition: fwd.hh:68
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:48
core::NumericalConstraints_t NumericalConstraints_t
Definition: fwd.hh:143
shared_ptr< const Problem > ProblemConstPtr_t
Definition: fwd.hh:66
constraints::ImplicitPtr_t ImplicitPtr_t
Definition: fwd.hh:131
core::size_type size_type
Definition: fwd.hh:90
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:49
Definition: main.hh:1