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;
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_;
Definition: states-path-finder.hh:99
std::vector< std::string > constraintNamesFromSolverAtWaypoint(std::size_t wp)
virtual void startSolve()
static StatesPathFinderPtr_t createWithRoadmap(const core::ProblemConstPtr_t &problem, const core::RoadmapPtr_t &roadmap)
SolveStepStatus solveStep(std::size_t wp)
StatesPathFinder(const StatesPathFinder &other)
void initWPRandom(std::size_t wp)
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)
void initWPNear(std::size_t wp)
virtual void tryConnectInitAndGoals()
std::vector< std::string > lastBuiltTransitions() const
void initWP(std::size_t wp, ConfigurationIn_t q)
std::string displayConfigsSolved() const
bool buildOptimizationProblemFromNames(std::vector< std::string > names)
void init(StatesPathFinderWkPtr_t weak)
Definition: states-path-finder.hh:162
StatesPathFinderPtr_t copy() const
#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
shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:65
shared_ptr< Roadmap > RoadmapPtr_t
Definition: fwd.hh:68
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