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 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
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:137
#define HPP_MANIPULATION_DLLAPI
Definition: config.hh:88
Definition: states-path-finder.hh:99
Solution has collision in edge going from the waypoint.
Definition: states-path-finder.hh:141
virtual ~StatesPathFinder()
Definition: states-path-finder.hh:103
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:139
core::size_type size_type
Definition: fwd.hh:90
Valid solution (no collision)
Definition: states-path-finder.hh:135
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:133
void init(StatesPathFinderWkPtr_t weak)
Definition: states-path-finder.hh:162