20 #ifndef HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH
21 #define HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH
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>
35 namespace manipulation {
36 namespace pathPlanner {
101 struct OptimizationData;
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);
129 void initWPRandom(std::size_t wp);
130 void initWPNear(std::size_t wp);
144 SolveStepStatus solveStep(std::size_t wp);
151 virtual void startSolve();
152 virtual void oneStep();
155 virtual void tryConnectInitAndGoals();
162 void init(StatesPathFinderWkPtr_t weak) { weak_ = weak; }
165 typedef constraints::solver::BySubstitution Solver_t;
166 struct GraphSearchData;
169 void gatherGraphConstraints();
173 bool findTransitions(GraphSearchData& data)
const;
177 const std::size_t& i)
const;
184 bool contains(
const Solver_t& solver,
const ImplicitPtr_t& c)
const;
190 bool containsStricter(
const Solver_t& solver,
const ImplicitPtr_t& c)
const;
191 bool checkConstantRightHandSide(
size_type index);
196 bool analyseOptimizationProblem(
const graph::Edges_t& transitions);
197 bool analyseOptimizationProblem2(
const graph::Edges_t& transitions,
201 void initializeRHS(std::size_t j);
202 bool solveOptimizationProblem();
205 core::Configurations_t getConfigList()
const;
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;
213 void displayRhsMatrix();
224 std::map<std::string, std::size_t> index_;
228 std::map<ImplicitPtr_t, ImplicitPtr_t> sameRightHandSide_;
233 std::map<ImplicitPtr_t, ImplicitPtr_t> stricterConstraints_;
235 mutable OptimizationData* optData_;
236 mutable std::shared_ptr<GraphSearchData> graphData_;
247 bool goalDefinedByConstraints_;
250 core::Configurations_t configList_;
251 std::size_t idxConfigList_;
253 bool solved_, interrupt_;
256 StatesPathFinderWkPtr_t weak_;
265 #endif // HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH