hpp-manipulation  5.0.0
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 
62  public:
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 
73  StateSelectorPtr_t createStateSelector(const std::string& name);
74 
79  void stateSelector(StateSelectorPtr_t ns);
80 
82  StateSelectorPtr_t stateSelector() const { return stateSelector_; }
83 
85  StatePtr_t getState(ConfigurationIn_t config) const;
86 
88  StatePtr_t getState(RoadmapNodePtr_t node) const;
89 
91  Edges_t getEdges(const StatePtr_t& from, const StatePtr_t& to) const;
92 
94  EdgePtr_t chooseEdge(RoadmapNodePtr_t node) const;
95 
98  void clearConstraintsAndComplement();
99 
113  void registerConstraints(const ImplicitPtr_t& constraint,
114  const ImplicitPtr_t& complement,
115  const ImplicitPtr_t& both);
116 
128  bool isComplement(const ImplicitPtr_t& constraint,
129  const ImplicitPtr_t& complement,
130  ImplicitPtr_t& combinationOfBoth) const;
131 
136  const ConstraintsAndComplements_t& constraintsAndComplements() const;
137 
141  ConstraintSetPtr_t configConstraint(const StatePtr_t& state) const;
142 
149  ConstraintSetPtr_t targetConstraint(const EdgePtr_t& edge) const;
150 
160  bool getConfigErrorForState(ConfigurationIn_t config, const StatePtr_t& state,
161  vector_t& error) const;
162 
170  // of the edge
174  bool getConfigErrorForEdge(ConfigurationIn_t config, const EdgePtr_t& edge,
175  vector_t& error) const;
176 
187  bool getConfigErrorForEdgeLeaf(ConfigurationIn_t leafConfig,
188  ConfigurationIn_t config,
189  const EdgePtr_t& edge, vector_t& error) const;
190 
201  bool getConfigErrorForEdgeTarget(ConfigurationIn_t leafConfig,
202  ConfigurationIn_t config,
203  const EdgePtr_t& edge,
204  vector_t& error) const;
205 
209  ConstraintSetPtr_t pathConstraint(const EdgePtr_t& edge) const;
210 
212  void maxIterations(size_type iterations);
213 
215  size_type maxIterations() const;
216 
218  void errorThreshold(const value_type& threshold);
219 
221  value_type errorThreshold() const;
222 
224  const DevicePtr_t& robot() const;
225 
227  const ProblemPtr_t& problem() const;
228 
230  void problem(const ProblemPtr_t& problem);
231 
234  hists_.push_back(hist);
235  }
236 
238  const Histograms_t& histograms() const { return hists_; }
239 
241  GraphComponentWkPtr_t get(std::size_t id) const;
242 
243  std::size_t nbComponents() const { return components_.size(); }
244 
246  virtual std::ostream& dotPrint(
247  std::ostream& os,
249 
251  virtual void initialize();
252 
254  virtual void invalidate();
255 
256  protected:
258  void init(const GraphWkPtr_t& weak, DevicePtr_t robot);
259 
262  Graph(const std::string& name, const ProblemPtr_t& problem);
263 
265  std::ostream& print(std::ostream& os) const;
266 
267  private:
269  GraphComponents_t& components();
270 
272  GraphComponents_t components_;
273 
275  StateSelectorPtr_t stateSelector_;
276 
279  ConstraintPtr_t constraints_;
280 
282  DevicePtr_t robot_;
283 
285  GraphWkPtr_t wkPtr_;
286 
288  typedef std::map<StatePtr_t, ConstraintSetPtr_t> MapFromState;
289  typedef std::pair<StatePtr_t, ConstraintSetPtr_t> PairStateConstraints;
290  MapFromState constraintSetMapFromState_;
291 
293  Histograms_t hists_;
294 
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 
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
std::vector< EdgePtr_t > Edges_t
Definition: fwd.hh:57
const Histograms_t & histograms() const
Get the histograms.
Definition: graph.hh:238
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:92
shared_ptr< Histogram > HistogramPtr_t
Definition: fwd.hh:69
core::value_type value_type
Definition: fwd.hh:89
void insertHistogram(const graph::HistogramPtr_t &hist)
Register an histogram representing a foliation.
Definition: graph.hh:233
std::vector< ConstraintAndComplement_t > ConstraintsAndComplements_t
Definition: constraint-set.hh:87
#define HPP_MANIPULATION_DLLAPI
Definition: config.hh:88
core::ConstraintPtr_t ConstraintPtr_t
Definition: fwd.hh:128
std::size_t nbComponents() const
Definition: graph.hh:243
std::list< HistogramPtr_t > Histograms_t
Definition: fwd.hh:73
StateSelectorPtr_t stateSelector() const
Get the state selector.
Definition: graph.hh:82
shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:65
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:138
constraints::ImplicitPtr_t ImplicitPtr_t
Definition: fwd.hh:131
core::size_type size_type
Definition: fwd.hh:90
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:61
Definition: roadmap-node.hh:42