hpp-manipulation 6.0.0
Classes for manipulation planning.
Loading...
Searching...
No Matches
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>
33#include <unordered_map>
34
35namespace hpp {
36namespace manipulation {
37namespace pathPlanner {
38
41
100class HPP_MANIPULATION_DLLAPI StatesPathFinder : public core::PathPlanner {
101 public:
102 struct OptimizationData;
103
104 virtual ~StatesPathFinder() {};
105
106 static StatesPathFinderPtr_t create(const core::ProblemConstPtr_t& problem);
107
109 const core::ProblemConstPtr_t& problem,
110 const core::RoadmapPtr_t& roadmap);
111
113
120 core::Configurations_t computeConfigList(ConfigurationIn_t q1,
122
123 // access functions for Python interface
124 std::vector<std::string> constraintNamesFromSolverAtWaypoint(std::size_t wp);
125 std::vector<std::string> lastBuiltTransitions() const;
126 std::string displayConfigsSolved() const;
127 bool buildOptimizationProblemFromNames(std::vector<std::string> names);
128
129 // Substeps of method solveOptimizationProblem
130 void initWPRandom(std::size_t wp);
131 void initWPNear(std::size_t wp);
132 void initWP(std::size_t wp, ConfigurationIn_t q);
144
145 SolveStepStatus solveStep(std::size_t wp);
146
150 void reset();
151
152 virtual void startSolve();
153 virtual void oneStep();
157
158 protected:
159 StatesPathFinder(const core::ProblemConstPtr_t& problem,
160 const core::RoadmapPtr_t&);
162
163 void init(StatesPathFinderWkPtr_t weak) { weak_ = weak; }
164
165 private:
166 typedef constraints::solver::BySubstitution Solver_t;
167 struct GraphSearchData;
168
170 void gatherGraphConstraints();
171
174 bool findTransitions(GraphSearchData& data) const;
175
177 graph::Edges_t getTransitionList(const GraphSearchData& data,
178 const std::size_t& i) const;
179
181
182 // check that the solver either contains exactly same constraint
183 // or a constraint with similar parametrizable form
184 // constraint/both and constraint/complement
185 bool contains(const Solver_t& solver, const ImplicitPtr_t& c) const;
186
187 // check that the solver either contains exactly same constraint
188 // or a stricter version of the constraint
189 // constraint/both stricter than constraint and stricter than
190 // constraint/complement
191 bool containsStricter(const Solver_t& solver, const ImplicitPtr_t& c) const;
192 bool checkConstantRightHandSide(size_type index);
193 bool buildOptimizationProblem(const graph::Edges_t& transitions);
194
196 void preInitializeRHS(std::size_t j, Configuration_t& q);
197 bool analyseOptimizationProblem(const graph::Edges_t& transitions,
198 core::ProblemConstPtr_t _problem);
199
201 void initializeRHS(std::size_t j);
202 bool solveOptimizationProblem();
203
204 // Data structure used to store a constraint right hand side as value and its
205 // name as key, both in form of hash numbers (so that names and rhs of two
206 // constraints can be easily merge). Exemple : ConstraintMap_t map =
207 // {{nameStringHash, rhsVectorHash}}; With rhsVectorHash the hash of a
208 // vector_t of rights hand side constraints with hashRHS, and nameStringHash
209 // the hash of a std::string - obtained for instance with std::hash.
210 typedef std::unordered_map<size_t, size_t> ConstraintMap_t; // map (name,
211 // rhs)
212
219 Configuration_t getConfigStatus(size_type i, size_type j) const;
220
226 vector_t getConstraintRHS(ImplicitPtr_t constraint, Configuration_t q) const;
227
231 size_t hashRHS(vector_t rhs) const;
232
240 bool checkSolvers(ConstraintMap_t const& pairMap,
241 ConstraintMap_t const& constraintMap) const;
242
251 bool saveIncompatibleRHS(ConstraintMap_t& pairMap,
252 ConstraintMap_t& constraintMap, size_type const wp);
253
254 // For a joint get his most, constrained with it, far parent
255 core::JointConstPtr_t maximalJoint(size_t const wp, core::JointConstPtr_t a);
256
258 core::Configurations_t getConfigList() const;
259
261 bool checkWaypointRightHandSide(std::size_t ictr, std::size_t jslv) const;
262 bool checkSolverRightHandSide(std::size_t ictr, std::size_t jslv) const;
263 bool checkWaypointRightHandSide(std::size_t jslv) const;
264 bool checkSolverRightHandSide(std::size_t jslv) const;
265
266 void displayRhsMatrix();
267 void displayStatusMatrix(const graph::Edges_t& transitions);
268
270 ProblemConstPtr_t problem_;
272 core::ProblemPtr_t inStateProblem_;
273
275 NumericalConstraints_t constraints_;
277 std::map<std::string, std::size_t> index_;
278
281 std::map<ImplicitPtr_t, ImplicitPtr_t> sameRightHandSide_;
282
286 std::map<ImplicitPtr_t, ImplicitPtr_t> stricterConstraints_;
287
288 mutable OptimizationData* optData_;
289 mutable std::shared_ptr<GraphSearchData> graphData_;
290 graph::Edges_t lastBuiltTransitions_;
291
299 NumericalConstraints_t goalConstraints_;
300 bool goalDefinedByConstraints_;
301 // Variables used across several calls to oneStep
302 Configuration_t q1_, q2_;
303 core::Configurations_t configList_;
304 std::size_t idxConfigList_;
305 size_type nTryConfigList_;
306 bool solved_, interrupt_;
307
309 StatesPathFinderWkPtr_t weak_;
310
311}; // class StatesPathFinder
313
314} // namespace pathPlanner
315} // namespace manipulation
316} // namespace hpp
317
318#endif // HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH
Definition states-path-finder.hh:100
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:104
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:134
@ VALID_SOLUTION
Valid solution (no collision)
Definition states-path-finder.hh:136
@ NO_SOLUTION
Bad solve status, no solution from the solver.
Definition states-path-finder.hh:138
@ COLLISION_AFTER
Solution has collision in edge going from the waypoint.
Definition states-path-finder.hh:142
@ COLLISION_BEFORE
Solution has collision in edge leading to the waypoint.
Definition states-path-finder.hh:140
static StatesPathFinderPtr_t create(const core::ProblemConstPtr_t &problem)
std::vector< std::string > constraintNamesFromSolverAtWaypoint(std::size_t wp)
void initWP(std::size_t wp, ConfigurationIn_t q)
std::vector< std::string > lastBuiltTransitions() const
bool buildOptimizationProblemFromNames(std::vector< std::string > names)
void init(StatesPathFinderWkPtr_t weak)
Definition states-path-finder.hh:163
#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
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
core::vector_t vector_t
Definition fwd.hh:92
Definition main.hh:1