hpp-manipulation 5.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
34namespace hpp {
35namespace manipulation {
36namespace pathPlanner {
37
40
99class HPP_MANIPULATION_DLLAPI StatesPathFinder : public core::PathPlanner {
100 public:
101 struct OptimizationData;
102
103 virtual ~StatesPathFinder(){};
104
105 static StatesPathFinderPtr_t create(const core::ProblemConstPtr_t& problem);
106
108 const core::ProblemConstPtr_t& problem,
109 const core::RoadmapPtr_t& roadmap);
110
112
119 core::Configurations_t computeConfigList(ConfigurationIn_t q1,
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();
156
157 protected:
158 StatesPathFinder(const core::ProblemConstPtr_t& problem,
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
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 > 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: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
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