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>
33 #include <unordered_map>
36 namespace manipulation {
37 namespace pathPlanner {
102 struct OptimizationData;
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);
130 void initWPRandom(std::size_t wp);
131 void initWPNear(std::size_t wp);
145 SolveStepStatus solveStep(std::size_t wp);
152 virtual void startSolve();
153 virtual void oneStep();
156 virtual void tryConnectInitAndGoals();
163 void init(StatesPathFinderWkPtr_t weak) { weak_ = weak; }
166 typedef constraints::solver::BySubstitution Solver_t;
167 struct GraphSearchData;
170 void gatherGraphConstraints();
174 bool findTransitions(GraphSearchData& data)
const;
178 const std::size_t& i)
const;
185 bool contains(
const Solver_t& solver,
const ImplicitPtr_t& c)
const;
191 bool containsStricter(
const Solver_t& solver,
const ImplicitPtr_t& c)
const;
192 bool checkConstantRightHandSide(
size_type index);
197 bool analyseOptimizationProblem(
const graph::Edges_t& transitions,
201 void initializeRHS(std::size_t j);
202 bool solveOptimizationProblem();
210 typedef std::unordered_map<size_t, size_t> ConstraintMap_t;
240 bool checkSolvers(ConstraintMap_t
const& pairMap,
241 ConstraintMap_t
const& constraintMap)
const;
251 bool saveIncompatibleRHS(ConstraintMap_t& pairMap,
252 ConstraintMap_t& constraintMap,
size_type const wp);
255 core::JointConstPtr_t maximalJoint(
size_t const wp, core::JointConstPtr_t a);
258 core::Configurations_t getConfigList()
const;
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;
266 void displayRhsMatrix();
277 std::map<std::string, std::size_t> index_;
281 std::map<ImplicitPtr_t, ImplicitPtr_t> sameRightHandSide_;
286 std::map<ImplicitPtr_t, ImplicitPtr_t> stricterConstraints_;
288 mutable OptimizationData* optData_;
289 mutable std::shared_ptr<GraphSearchData> graphData_;
300 bool goalDefinedByConstraints_;
303 core::Configurations_t configList_;
304 std::size_t idxConfigList_;
306 bool solved_, interrupt_;
309 StatesPathFinderWkPtr_t weak_;
318 #endif // HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH