GCC Code Coverage Report

Directory: ./
File: include/hpp/manipulation/graph/graph.hh
Date: 2025-03-07 11:10:46
Exec Total Coverage
Lines: 3 8 37.5%
Branches: 0 0 -%
Line Branch Exec Source
1 // Copyright (c) 2014, LAAS-CNRS
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #ifndef HPP_MANIPULATION_GRAPH_GRAPH_HH
30 #define HPP_MANIPULATION_GRAPH_GRAPH_HH
31
32 #include <tuple>
33
34 #include "hpp/manipulation/config.hh"
35 #include "hpp/manipulation/constraint-set.hh"
36 #include "hpp/manipulation/fwd.hh"
37 #include "hpp/manipulation/graph/fwd.hh"
38 #include "hpp/manipulation/graph/graph-component.hh"
39
40 namespace hpp {
41 namespace manipulation {
42 typedef constraints::ImplicitPtr_t ImplicitPtr_t;
43 namespace graph {
44 /// \addtogroup constraint_graph
45 /// \{
46
47 /// Description of the constraint graph.
48 ///
49 /// This class contains a graph representing a a manipulation problem
50 ///
51 /// One must make sure not to create loop with shared pointers.
52 /// To ensure that, the classes are defined as follow:
53 /// - A Graph owns (i.e. has a shared pointer to) the StateSelector s
54 /// - A StateSelector owns the Node s related to one gripper.
55 /// - A State owns its outgoing Edge s.
56 /// - An Edge does not own anything.
57 ///
58 /// \note The graph and all its components have a unique index starting
59 /// at 0 for the graph itself. The index of a component can be retrieved
60 /// using method GraphComponent::id.
61 class HPP_MANIPULATION_DLLAPI Graph : public GraphComponent {
62 public:
63 /// Create a new Graph.
64 ///
65 /// \param robot a manipulation robot
66 /// \param problem a pointer to the problem
67 static GraphPtr_t create(const std::string& name, DevicePtr_t robot,
68 const ProblemPtr_t& problem);
69
70 ✗ GraphPtr_t self() const { return wkPtr_.lock(); }
71
72 /// Create and insert a state selector inside the graph.
73 StateSelectorPtr_t createStateSelector(const std::string& name);
74
75 /// Set the state selector
76 /// \warning This should be done before adding nodes to the node
77 /// selector otherwise the pointer to the parent graph will NOT be
78 /// valid.
79 void stateSelector(StateSelectorPtr_t ns);
80
81 /// Get the state selector
82 ✗ StateSelectorPtr_t stateSelector() const { return stateSelector_; }
83
84 /// Returns the state of a configuration.
85 StatePtr_t getState(ConfigurationIn_t config) const;
86
87 /// Returns the state of a roadmap node
88 StatePtr_t getState(RoadmapNodePtr_t node) const;
89
90 /// Get possible edges between two nodes.
91 Edges_t getEdges(const StatePtr_t& from, const StatePtr_t& to) const;
92
93 /// Select randomly outgoing edge of the given node.
94 EdgePtr_t chooseEdge(RoadmapNodePtr_t node) const;
95
96 /// Clear the vector of constraints and complements
97 /// \sa registerConstraints
98 void clearConstraintsAndComplement();
99
100 /// Register a triple of constraints to be inserted in nodes and edges
101 /// \param constraint a constraint (grasp of placement)
102 /// \param complement the complement constraint
103 /// \param both combination of the constraint and its complement. Both
104 /// constraints together corresponds to a full relative
105 /// transformation constraint
106 /// When inserting constraints in transitions of the graph,
107 /// in many cases, a constraint is associated to a state and
108 /// the complement constraint is associated to the
109 /// transition itself. Registering those constraints
110 /// priorly to graph construction makes possible to replace
111 /// the constraint and its complement by the combination of
112 /// both that is an explicit constraint.
113 void registerConstraints(const ImplicitPtr_t& constraint,
114 const ImplicitPtr_t& complement,
115 const ImplicitPtr_t& both);
116
117 /// Test whether two constraints are complement of one another
118 ///
119 /// \param constraint, complement two constraints to test
120 /// \retval combinationOfBoth constraint corresponding to combining
121 /// constraint and complement if result is true,
122 /// unchanged otherwise.
123 /// \return whether complement is the complement of constraint.
124 /// Two constraints are complement of one another if and only if
125 /// combined they constitute a complement relative transformation
126 /// constraint. \sa Graph::registerConstraints
127 /// \warning argument order matters.
128 bool isComplement(const ImplicitPtr_t& constraint,
129 const ImplicitPtr_t& complement,
130 ImplicitPtr_t& combinationOfBoth) const;
131
132 /// Return the vector of tuples as registered in registerConstraints
133 /// \return a vector of tuples (c, c/complement, c/both) each of them
134 /// corresponding to a constraint, the complement constraint
135 /// and the combination of boths.
136 const ConstraintsAndComplements_t& constraintsAndComplements() const;
137
138 /// Constraint to project onto the Node.
139 /// \param state the state on which to project.
140 /// \return The initialized projector.
141 ConstraintSetPtr_t configConstraint(const StatePtr_t& state) const;
142
143 /// Constraints a configuration in target state should satisfy
144 /// \param edge a transition
145 /// \return The set of constraints a configuration lying in the
146 /// target state of the edge should satisfy. This set
147 /// includes the paths constraints of the edge.
148 /// \sa Edge::targetConstraint.
149 ConstraintSetPtr_t targetConstraint(const EdgePtr_t& edge) const;
150
151 /// Get error of a config with respect to a state constraint
152 ///
153 /// \param config Configuration,
154 /// \param state state containing the constraint to check config against
155 /// \retval error the error of the state constraint for the
156 /// configuration
157 /// \return whether the configuration belongs to the state.
158 /// Call method core::ConstraintSet::isSatisfied for the state
159 /// constraints.
160 bool getConfigErrorForState(ConfigurationIn_t config, const StatePtr_t& state,
161 vector_t& error) const;
162
163 /// Get error of a config with respect to an edge constraint
164 ///
165 /// \param config Configuration,
166 /// \param edge edge containing the constraint to check config against
167 /// \retval error the error of the edge constraint for the
168 /// configuration
169 /// \return whether the configuration can be a start point of a path
170 // of the edge
171 /// Call core::ConfigProjector::rightHandSideFromConfig with
172 /// input configuration and method core::ConstraintSet::isSatisfied
173 /// for the edge constraint.
174 bool getConfigErrorForEdge(ConfigurationIn_t config, const EdgePtr_t& edge,
175 vector_t& error) const;
176
177 /// Get error of a config with respect to an edge foliation leaf
178 ///
179 /// \param leafConfig Configuration that determines the foliation leaf
180 /// \param config Configuration the error of which is computed
181 /// \retval error the error
182 /// \return whether config can be the end point of a path of the edge
183 /// starting at leafConfig
184 /// Call methods core::ConfigProjector::rightHandSideFromConfig with
185 /// leafConfig and then core::ConstraintSet::isSatisfied with config.
186 /// on the edge constraints.
187 bool getConfigErrorForEdgeLeaf(ConfigurationIn_t leafConfig,
188 ConfigurationIn_t config,
189 const EdgePtr_t& edge, vector_t& error) const;
190
191 /// Get error of a config with respect to the target of an edge foliation leaf
192 ///
193 /// \param leafConfig Configuration that determines the foliation leaf
194 /// \param config Configuration the error of which is computed
195 /// \retval error the error
196 /// \return whether config can be the end point of a path of the edge
197 /// starting at leafConfig
198 /// Call methods core::ConfigProjector::rightHandSideFromConfig with
199 /// leafConfig and then core::ConstraintSet::isSatisfied with config.
200 /// on the edge constraints.
201 bool getConfigErrorForEdgeTarget(ConfigurationIn_t leafConfig,
202 ConfigurationIn_t config,
203 const EdgePtr_t& edge,
204 vector_t& error) const;
205
206 /// Constraint to project a path.
207 /// \param edge a list of edges defining the foliation.
208 /// \return The constraint.
209 ConstraintSetPtr_t pathConstraint(const EdgePtr_t& edge) const;
210
211 /// Set maximal number of iterations
212 void maxIterations(size_type iterations);
213
214 /// Get maximal number of iterations in config projector
215 size_type maxIterations() const;
216
217 /// Set error threshold
218 void errorThreshold(const value_type& threshold);
219
220 /// Get error threshold in config projector
221 value_type errorThreshold() const;
222
223 /// Get the robot.
224 const DevicePtr_t& robot() const;
225
226 /// Get the problem
227 const ProblemPtr_t& problem() const;
228
229 /// Set the problem
230 void problem(const ProblemPtr_t& problem);
231
232 /// Register an histogram representing a foliation
233 2 void insertHistogram(const graph::HistogramPtr_t& hist) {
234 2 hists_.push_back(hist);
235 2 }
236
237 /// Get the histograms
238 ✗ const Histograms_t& histograms() const { return hists_; }
239
240 /// Get the component by its ID.
241 GraphComponentWkPtr_t get(std::size_t id) const;
242
243 ✗ std::size_t nbComponents() const { return components_.size(); }
244
245 /// Print the component in DOT language.
246 virtual std::ostream& dotPrint(
247 std::ostream& os,
248 dot::DrawingAttributes da = dot::DrawingAttributes()) const;
249
250 /// Initialize all components of the graph (edges and states)
251 virtual void initialize();
252
253 /// Invalidate all states and edges of the graph
254 virtual void invalidate();
255
256 protected:
257 /// Initialization of the object.
258 void init(const GraphWkPtr_t& weak, DevicePtr_t robot);
259
260 /// Constructor
261 /// \param problem a pointer to the problem
262 Graph(const std::string& name, const ProblemPtr_t& problem);
263
264 /// Print the object in a stream.
265 std::ostream& print(std::ostream& os) const;
266
267 private:
268 /// The list of elements
269 GraphComponents_t& components();
270
271 /// Keep track of the created components
272 GraphComponents_t components_;
273
274 /// This list contains a state selector for each end-effector.
275 StateSelectorPtr_t stateSelector_;
276
277 /// A set of constraints that will always be used, for example
278 /// stability constraints.
279 ConstraintPtr_t constraints_;
280
281 /// Keep a pointer to the composite robot.
282 DevicePtr_t robot_;
283
284 /// Weak pointer to itself.
285 GraphWkPtr_t wkPtr_;
286
287 /// Map of constraint sets (from State).
288 typedef std::map<StatePtr_t, ConstraintSetPtr_t> MapFromState;
289 typedef std::pair<StatePtr_t, ConstraintSetPtr_t> PairStateConstraints;
290 MapFromState constraintSetMapFromState_;
291
292 /// List of histograms
293 Histograms_t hists_;
294
295 /// Map of constraint sets (from Edge).
296 typedef std::map<EdgePtr_t, ConstraintSetPtr_t> MapFromEdge;
297 typedef std::pair<EdgePtr_t, ConstraintSetPtr_t> PairEdgeConstraints;
298 MapFromEdge cfgConstraintSetMapFromEdge_, pathConstraintSetMapFromEdge_;
299 ProblemPtr_t problem_;
300 value_type errorThreshold_;
301 size_type maxIterations_;
302
303 ConstraintsAndComplements_t constraintsAndComplements_;
304 friend class GraphComponent;
305 }; // Class Graph
306
307 /// \}
308 } // namespace graph
309 } // namespace manipulation
310
311 } // namespace hpp
312
313 ✗ BOOST_CLASS_EXPORT_KEY(hpp::manipulation::graph::Graph)
314
315 #endif // HPP_MANIPULATION_GRAPH_GRAPH_HH
316