hpp-manipulation  4.15.1
Classes for manipulation planning.
graph.hh
Go to the documentation of this file.
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 
36 #include "hpp/manipulation/fwd.hh"
39 
40 namespace hpp {
41 namespace manipulation {
43 namespace graph {
46 
59  public:
64  static GraphPtr_t create(const std::string& name, DevicePtr_t robot,
65  const ProblemPtr_t& problem);
66 
67  GraphPtr_t self() const { return wkPtr_.lock(); }
68 
70  StateSelectorPtr_t createStateSelector(const std::string& name);
71 
76  void stateSelector(StateSelectorPtr_t ns);
77 
79  StateSelectorPtr_t stateSelector() const { return stateSelector_; }
80 
82  StatePtr_t getState(ConfigurationIn_t config) const;
83 
85  StatePtr_t getState(RoadmapNodePtr_t node) const;
86 
88  Edges_t getEdges(const StatePtr_t& from, const StatePtr_t& to) const;
89 
91  EdgePtr_t chooseEdge(RoadmapNodePtr_t node) const;
92 
95  void clearConstraintsAndComplement();
96 
110  void registerConstraints(const ImplicitPtr_t& constraint,
111  const ImplicitPtr_t& complement,
112  const ImplicitPtr_t& both);
113 
125  bool isComplement(const ImplicitPtr_t& constraint,
126  const ImplicitPtr_t& complement,
127  ImplicitPtr_t& combinationOfBoth) const;
128 
133  const ConstraintsAndComplements_t& constraintsAndComplements() const;
134 
138  ConstraintSetPtr_t configConstraint(const StatePtr_t& state) const;
139 
142  ConstraintSetPtr_t configConstraint(const EdgePtr_t& edge) const
144 
151  ConstraintSetPtr_t targetConstraint(const EdgePtr_t& edge) const;
152 
162  bool getConfigErrorForState(ConfigurationIn_t config, const StatePtr_t& state,
163  vector_t& error) const;
164 
172  // of the edge
176  bool getConfigErrorForEdge(ConfigurationIn_t config, const EdgePtr_t& edge,
177  vector_t& error) const;
178 
189  bool getConfigErrorForEdgeLeaf(ConfigurationIn_t leafConfig,
190  ConfigurationIn_t config,
191  const EdgePtr_t& edge, vector_t& error) const;
192 
203  bool getConfigErrorForEdgeTarget(ConfigurationIn_t leafConfig,
204  ConfigurationIn_t config,
205  const EdgePtr_t& edge,
206  vector_t& error) const;
207 
211  ConstraintSetPtr_t pathConstraint(const EdgePtr_t& edge) const;
212 
214  void maxIterations(size_type iterations);
215 
217  size_type maxIterations() const;
218 
220  void errorThreshold(const value_type& threshold);
221 
223  value_type errorThreshold() const;
224 
226  const DevicePtr_t& robot() const;
227 
229  const ProblemPtr_t& problem() const;
230 
232  void problem(const ProblemPtr_t& problem);
233 
236  hists_.push_back(hist);
237  }
238 
240  const Histograms_t& histograms() const { return hists_; }
241 
243  GraphComponentWkPtr_t get(std::size_t id) const;
244 
245  std::size_t nbComponents() const { return components_.size(); }
246 
248  virtual std::ostream& dotPrint(
249  std::ostream& os,
251 
253  virtual void initialize();
254 
256  virtual void invalidate();
257 
258  protected:
260  void init(const GraphWkPtr_t& weak, DevicePtr_t robot);
261 
264  Graph(const std::string& name, const ProblemPtr_t& problem);
265 
267  std::ostream& print(std::ostream& os) const;
268 
269  private:
271  GraphComponents_t& components();
272 
274  GraphComponents_t components_;
275 
277  StateSelectorPtr_t stateSelector_;
278 
281  ConstraintPtr_t constraints_;
282 
284  DevicePtr_t robot_;
285 
287  GraphWkPtr_t wkPtr_;
288 
290  typedef std::map<StatePtr_t, ConstraintSetPtr_t> MapFromState;
291  typedef std::pair<StatePtr_t, ConstraintSetPtr_t> PairStateConstraints;
292  MapFromState constraintSetMapFromState_;
293 
295  Histograms_t hists_;
296 
298  typedef std::map<EdgePtr_t, ConstraintSetPtr_t> MapFromEdge;
299  typedef std::pair<EdgePtr_t, ConstraintSetPtr_t> PairEdgeConstraints;
300  MapFromEdge cfgConstraintSetMapFromEdge_, pathConstraintSetMapFromEdge_;
301  ProblemPtr_t problem_;
302  value_type errorThreshold_;
303  size_type maxIterations_;
304 
305  ConstraintsAndComplements_t constraintsAndComplements_;
306  friend class GraphComponent;
307 }; // Class Graph
308 
310 } // namespace graph
311 } // namespace manipulation
312 
313 } // namespace hpp
314 
315 BOOST_CLASS_EXPORT_KEY(hpp::manipulation::graph::Graph)
316 
317 #endif // HPP_MANIPULATION_GRAPH_GRAPH_HH
std::vector< EdgePtr_t > Edges_t
Definition: fwd.hh:57
StateHistogram NodeHistogram HPP_MANIPULATION_DEPRECATED
Definition: statistics.hh:206
const Histograms_t & histograms() const
Get the histograms.
Definition: graph.hh:240
shared_ptr< StateSelector > StateSelectorPtr_t
Definition: fwd.hh:52
Definition: main.hh:1
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:49
core::vector_t vector_t
Definition: fwd.hh:93
shared_ptr< Histogram > HistogramPtr_t
Definition: fwd.hh:69
core::value_type value_type
Definition: fwd.hh:90
void insertHistogram(const graph::HistogramPtr_t &hist)
Register an histogram representing a foliation.
Definition: graph.hh:235
std::vector< ConstraintAndComplement_t > ConstraintsAndComplements_t
Definition: constraint-set.hh:87
#define HPP_MANIPULATION_DLLAPI
Definition: config.hh:64
core::ConstraintPtr_t ConstraintPtr_t
Definition: fwd.hh:113
std::size_t nbComponents() const
Definition: graph.hh:245
std::list< HistogramPtr_t > Histograms_t
Definition: fwd.hh:73
StateSelectorPtr_t stateSelector() const
Get the state selector.
Definition: graph.hh:79
shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:66
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:123
constraints::ImplicitPtr_t ImplicitPtr_t
Definition: fwd.hh:116
core::size_type size_type
Definition: fwd.hh:91
shared_ptr< Device > DevicePtr_t
Definition: fwd.hh:40
shared_ptr< Graph > GraphPtr_t
Definition: fwd.hh:47
Define common methods of the graph components.
Definition: graph-component.hh:51
std::vector< GraphComponentWkPtr_t > GraphComponents_t
Definition: fwd.hh:55
shared_ptr< Edge > EdgePtr_t
Definition: fwd.hh:49
shared_ptr< State > StatePtr_t
Definition: fwd.hh:48
Definition: graph.hh:58
Definition: roadmap-node.hh:42