GCC Code Coverage Report

Directory: ./
File: include/hpp/manipulation/path-planner/states-path-finder.hh
Date: 2025-03-07 11:10:46
Exec Total Coverage
Lines: 0 2 0.0%
Branches: 0 0 -%
Line Branch Exec Source
1 // Copyright (c) 2017, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr),
3 // Florent Lamiraux (florent.lamiraux@laas.fr)
4 // Alexandre Thiault (athiault@laas.fr)
5 // Le Quang Anh (quang-anh.le@laas.fr)
6 //
7 // This file is part of hpp-manipulation.
8 // hpp-manipulation is free software: you can redistribute it
9 // and/or modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation, either version
11 // 3 of the License, or (at your option) any later version.
12 //
13 // hpp-manipulation is distributed in the hope that it will be
14 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
15 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // General Lesser Public License for more details. You should have
17 // received a copy of the GNU Lesser General Public License along with
18 // hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
19
20 #ifndef HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH
21 #define HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH
22
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>
30 #include <hpp/manipulation/config.hh>
31 #include <hpp/manipulation/fwd.hh>
32 #include <hpp/manipulation/problem.hh>
33 #include <unordered_map>
34
35 namespace hpp {
36 namespace manipulation {
37 namespace pathPlanner {
38
39 /// \addtogroup path_planner
40 /// \{
41
42 /// Optimization-based path planning method.
43 ///
44 /// #### Sketch of the method
45 ///
46 /// Given two configurations \f$ (q_1,q_2) \f$, this class formulates and
47 /// solves the problem as follows.
48 /// - Compute the corresponding states \f$ (s_1, s_2) \f$.
49 /// - For each path \f$ (e_0, ... e_n) \f$ of length not more than
50 /// parameter "StatesPathFinder/maxDepth" between
51 /// \f$ (s_1, s_2)\f$ in the constraint graph, do:
52 /// - define \f$ n-1 \f$ intermediate configuration \f$ p_i \f$,
53 /// - initialize the optimization problem, as explained below,
54 /// - solve the optimization problem, which gives \f$ p^*_i \f$,
55 /// - in case of failure, continue the loop.
56 ///
57 /// #### Problem formulation
58 /// Find \f$ (p_i) \f$ such that:
59 /// - \f$ p_0 = q_1 \f$,
60 /// - \f$ p_{n+1} = q_2 \f$,
61 /// - \f$ p_i \f$ is in state between \f$ (e_{i-1}, e_i) \f$, (\ref
62 /// StateFunction)
63 /// - \f$ (p_i, p_{i+1}) \f$ are reachable with transition \f$ e_i \f$ (\ref
64 /// EdgeFunction).
65 ///
66 /// #### Problem resolution
67 ///
68 /// One solver (hpp::constraints::solver::BySubstitution) is created
69 /// for each waypoint \f$p_i\f$.
70 /// - method buildOptimizationProblem builds a matrix the rows of which
71 /// are the parameterizable numerical constraints present in the
72 /// graph, and the columns of which are the waypoints. Each value in the
73 /// matrix defines the status of each constraint right hand side for
74 /// this waypoint, among {absent from the solver,
75 /// equal to value for previous waypoint,
76 /// equal to value for start configuration,
77 /// equal to value for end configuration}.
78 /// - method analyseOptimizationProblem loops over the waypoint solvers,
79 /// tests what happens when solving each waypoint after initializing
80 /// only the right hand sides that are equal to the initial or goal
81 /// configuration, and detects if a collision is certain to block any attempts
82 /// to solve the problem in the solveOptimizationProblem step.
83 /// - method solveOptimizationProblem tries to solve for each waypoint after
84 /// initializing the right hand sides with the proper values, backtracking
85 /// to the previous waypoint if the solving failed or a collision is
86 /// detected a number of times set from the parameter
87 /// "StatesPathFinder/nTriesUntilBacktrack". If too much backtracking
88 /// occurs, the method can eventually return false.
89 /// - eventually method buildPath build paths between waypoints with
90 /// the constraints of the transition in which the path lies.
91 ///
92 /// #### Current status
93 ///
94 /// The method has been successfully tested with romeo holding a placard
95 /// and the construction set benchmarks. The result is satisfactory
96 /// except between pregrasp and grasp waypoints that may be far
97 /// away from each other if the transition between those state does
98 /// not contain the grasp complement constraint. The same holds
99 /// between placement and pre-placement.
100 class HPP_MANIPULATION_DLLAPI StatesPathFinder : public core::PathPlanner {
101 public:
102 struct OptimizationData;
103
104 ✗ virtual ~StatesPathFinder() {};
105
106 static StatesPathFinderPtr_t create(const core::ProblemConstPtr_t& problem);
107
108 static StatesPathFinderPtr_t createWithRoadmap(
109 const core::ProblemConstPtr_t& problem,
110 const core::RoadmapPtr_t& roadmap);
111
112 StatesPathFinderPtr_t copy() const;
113
114 /// create a vector of configurations between two configurations
115 /// \param q1 initial configuration
116 /// \param q2 pointer to final configuration, NULL if goal is
117 /// defined as a set of constraints
118 /// \return a Configurations_t from q1 to q2 if found. An empty
119 /// vector if a path could not be built.
120 core::Configurations_t computeConfigList(ConfigurationIn_t q1,
121 ConfigurationIn_t q2);
122
123 // access functions for Python interface
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);
128
129 // Substeps of method solveOptimizationProblem
130 void initWPRandom(std::size_t wp);
131 void initWPNear(std::size_t wp);
132 void initWP(std::size_t wp, ConfigurationIn_t q);
133 /// Status of the step to solve for a particular waypoint
134 enum SolveStepStatus {
135 /// Valid solution (no collision)
136 VALID_SOLUTION,
137 /// Bad solve status, no solution from the solver
138 NO_SOLUTION,
139 /// Solution has collision in edge leading to the waypoint
140 COLLISION_BEFORE,
141 /// Solution has collision in edge going from the waypoint
142 COLLISION_AFTER,
143 };
144
145 SolveStepStatus solveStep(std::size_t wp);
146
147 /// deletes from memory the latest working states list, which is used to
148 /// resume finding solutions from that list in case of failure at a
149 /// later step.
150 void reset();
151
152 virtual void startSolve();
153 virtual void oneStep();
154 /// when both initial state is one of potential goal states,
155 /// try connecting them directly
156 virtual void tryConnectInitAndGoals();
157
158 protected:
159 StatesPathFinder(const core::ProblemConstPtr_t& problem,
160 const core::RoadmapPtr_t&);
161 StatesPathFinder(const StatesPathFinder& other);
162
163 ✗ void init(StatesPathFinderWkPtr_t weak) { weak_ = weak; }
164
165 private:
166 typedef constraints::solver::BySubstitution Solver_t;
167 struct GraphSearchData;
168
169 /// Gather constraints of all edges
170 void gatherGraphConstraints();
171
172 /// Step 1 of the algorithm
173 /// \return whether the max depth was reached.
174 bool findTransitions(GraphSearchData& data) const;
175
176 /// Step 2 of the algorithm
177 graph::Edges_t getTransitionList(const GraphSearchData& data,
178 const std::size_t& i) const;
179
180 /// Step 3 of the algorithm
181
182 // check that the solver either contains exactly same constraint
183 // or a constraint with similar parametrizable form
184 // constraint/both and constraint/complement
185 bool contains(const Solver_t& solver, const ImplicitPtr_t& c) const;
186
187 // check that the solver either contains exactly same constraint
188 // or a stricter version of the constraint
189 // constraint/both stricter than constraint and stricter than
190 // constraint/complement
191 bool containsStricter(const Solver_t& solver, const ImplicitPtr_t& c) const;
192 bool checkConstantRightHandSide(size_type index);
193 bool buildOptimizationProblem(const graph::Edges_t& transitions);
194
195 /// Step 4 of the algorithm
196 void preInitializeRHS(std::size_t j, Configuration_t& q);
197 bool analyseOptimizationProblem(const graph::Edges_t& transitions,
198 core::ProblemConstPtr_t _problem);
199
200 /// Step 5 of the algorithm
201 void initializeRHS(std::size_t j);
202 bool solveOptimizationProblem();
203
204 // Data structure used to store a constraint right hand side as value and its
205 // name as key, both in form of hash numbers (so that names and rhs of two
206 // constraints can be easily merge). Exemple : ConstraintMap_t map =
207 // {{nameStringHash, rhsVectorHash}}; With rhsVectorHash the hash of a
208 // vector_t of rights hand side constraints with hashRHS, and nameStringHash
209 // the hash of a std::string - obtained for instance with std::hash.
210 typedef std::unordered_map<size_t, size_t> ConstraintMap_t; // map (name,
211 // rhs)
212
213 /// @brief Get a configuration in accordance with the statuts matrix at a step
214 /// j for the constraint i
215 /// @param i number of the constraint in the status matrix
216 /// @param j step of the potential solution (index of a waypoint)
217 /// @return a configuration Configuration_t which follows the status matrix
218 /// indication at the given indices
219 Configuration_t getConfigStatus(size_type i, size_type j) const;
220
221 /// @brief Get the right hand side of a constraint w.r.t a set configuration
222 /// for this constraint
223 /// @param constraint the constraint to compute the right hand side of
224 /// @param q the configuration in which the constraint is set
225 /// @return a right hand side vector_t
226 vector_t getConstraintRHS(ImplicitPtr_t constraint, Configuration_t q) const;
227
228 /// @brief Hash a vector of right hand side into a long unsigned integer
229 /// @param rhs the right hand side vector vector_t
230 /// @return a size_t integer hash
231 size_t hashRHS(vector_t rhs) const;
232
233 /// @brief Check if a solution (a list of transition) contains impossible to
234 /// solve steps due to inevitable collisions
235 /// @param pairMap The ConstraintMap_tf table of pairs of incompatibles
236 /// constraints
237 /// @param constraintMap The hasmap table of constraints which are in pairMap
238 /// @return a bool which is true is there is no impossible to solve steps,
239 /// true otherwise
240 bool checkSolvers(ConstraintMap_t const& pairMap,
241 ConstraintMap_t const& constraintMap) const;
242
243 /// @brief For a certain step wp during solving check for collision impossible
244 /// to solve.
245 /// @param pairMap The ConstraintMap_t table of pairs of incompatibles
246 /// constraints
247 /// @param constraintMap The hasmap table of constraints which are in pairMap
248 /// @param wp The index of the current step
249 /// @return a bool which is true if there is no collision or impossible to
250 /// solve ones, false otherwise.
251 bool saveIncompatibleRHS(ConstraintMap_t& pairMap,
252 ConstraintMap_t& constraintMap, size_type const wp);
253
254 // For a joint get his most, constrained with it, far parent
255 core::JointConstPtr_t maximalJoint(size_t const wp, core::JointConstPtr_t a);
256
257 /// Step 6 of the algorithm
258 core::Configurations_t getConfigList() const;
259
260 /// Functions used in assert statements
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;
265
266 void displayRhsMatrix();
267 void displayStatusMatrix(const graph::Edges_t& transitions);
268
269 /// A pointer to the manipulation problem
270 ProblemConstPtr_t problem_;
271 /// Path planning problem in each leaf.
272 core::ProblemPtr_t inStateProblem_;
273
274 /// Vector of parameterizable edge numerical constraints
275 NumericalConstraints_t constraints_;
276 /// Map of indices in constraints_
277 std::map<std::string, std::size_t> index_;
278
279 /// associative map that stores pairs of constraints of the form
280 /// (constraint/complement, constraint/hold)
281 std::map<ImplicitPtr_t, ImplicitPtr_t> sameRightHandSide_;
282
283 /// associative map that stores pairs of constraints of either form
284 /// (constraint, constraint/hold)
285 /// or (constraint/complement, constraint/hold)
286 std::map<ImplicitPtr_t, ImplicitPtr_t> stricterConstraints_;
287
288 mutable OptimizationData* optData_;
289 mutable std::shared_ptr<GraphSearchData> graphData_;
290 graph::Edges_t lastBuiltTransitions_;
291
292 /// Constraints defining the goal
293 /// For now:
294 /// - comparison type Equality is initialized to zero
295 /// - if goal constraint is not already present in any graph state,
296 /// it should not require propagating a complement.
297 /// invalid eg: specify the full pose of an object placement or
298 /// object grasp
299 NumericalConstraints_t goalConstraints_;
300 bool goalDefinedByConstraints_;
301 // Variables used across several calls to oneStep
302 Configuration_t q1_, q2_;
303 core::Configurations_t configList_;
304 std::size_t idxConfigList_;
305 size_type nTryConfigList_;
306 bool solved_, interrupt_;
307
308 /// Weak pointer to itself
309 StatesPathFinderWkPtr_t weak_;
310
311 }; // class StatesPathFinder
312 /// \}
313
314 } // namespace pathPlanner
315 } // namespace manipulation
316 } // namespace hpp
317
318 #endif // HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH
319