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 std::vector< EdgePtr_t > Edges_t
Definition: fwd.hh:57
shared_ptr< Roadmap > RoadmapPtr_t
Definition: fwd.hh:68
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:49
core::vector_t vector_t
Definition: fwd.hh:92
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:48
shared_ptr< const Problem > ProblemConstPtr_t
Definition: fwd.hh:66
Bad solve status, no solution from the solver.
Definition: states-path-finder.hh:138
#define HPP_MANIPULATION_DLLAPI
Definition: config.hh:88
Definition: states-path-finder.hh:100
Solution has collision in edge going from the waypoint.
Definition: states-path-finder.hh:142
virtual ~StatesPathFinder()
Definition: states-path-finder.hh:104
shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:65
constraints::ImplicitPtr_t ImplicitPtr_t
Definition: fwd.hh:131
core::NumericalConstraints_t NumericalConstraints_t
Definition: fwd.hh:143
Solution has collision in edge leading to the waypoint.
Definition: states-path-finder.hh:140
core::size_type size_type
Definition: fwd.hh:90
Valid solution (no collision)
Definition: states-path-finder.hh:136
shared_ptr< StatesPathFinder > StatesPathFinderPtr_t
Definition: fwd.hh:101
SolveStepStatus
Status of the step to solve for a particular waypoint.
Definition: states-path-finder.hh:134
void init(StatesPathFinderWkPtr_t weak)
Definition: states-path-finder.hh:163