hpp-manipulation  4.12.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 // This file is part of hpp-manipulation.
5 // hpp-manipulation is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // hpp-manipulation is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
16 
17 #ifndef HPP_MANIPULATION_GRAPH_GRAPH_HH
18 # define HPP_MANIPULATION_GRAPH_GRAPH_HH
19 
20 # include <tuple>
23 # include "hpp/manipulation/fwd.hh"
26 
27 namespace hpp {
28  namespace manipulation {
30  namespace graph {
33 
46  {
47  public:
52  static GraphPtr_t create(const std::string& name, DevicePtr_t robot,
53  const ProblemPtr_t& problem);
54 
55  GraphPtr_t self () const { return wkPtr_.lock(); }
56 
58  StateSelectorPtr_t createStateSelector (const std::string& name);
59 
64  void stateSelector (StateSelectorPtr_t ns);
65 
68  {
69  return stateSelector_;
70  }
71 
73  StatePtr_t getState (ConfigurationIn_t config) const;
74 
76  StatePtr_t getState (RoadmapNodePtr_t node) const;
77 
79  Edges_t getEdges (const StatePtr_t& from, const StatePtr_t& to) const;
80 
82  EdgePtr_t chooseEdge(RoadmapNodePtr_t node) const;
83 
86  void clearConstraintsAndComplement();
87 
101  void registerConstraints (const ImplicitPtr_t& constraint,
102  const ImplicitPtr_t& complement,
103  const ImplicitPtr_t& both);
104 
116  bool isComplement (const ImplicitPtr_t& constraint,
117  const ImplicitPtr_t& complement,
118  ImplicitPtr_t& combinationOfBoth) const;
119 
124  const ConstraintsAndComplements_t& constraintsAndComplements () const;
125 
129  ConstraintSetPtr_t configConstraint (const StatePtr_t& state) const;
130 
133  ConstraintSetPtr_t configConstraint (const EdgePtr_t& edge) const
135 
142  ConstraintSetPtr_t targetConstraint (const EdgePtr_t& edge) const;
143 
153  bool getConfigErrorForState (ConfigurationIn_t config,
154  const StatePtr_t& state, vector_t& error) const;
155 
163  // of the edge
167  bool getConfigErrorForEdge (ConfigurationIn_t config,
168  const EdgePtr_t& edge, vector_t& error) const;
169 
180  bool getConfigErrorForEdgeLeaf
181  (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
182  const EdgePtr_t& edge, vector_t& error) const;
183 
194  bool getConfigErrorForEdgeTarget
195  (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
196  const EdgePtr_t& edge, vector_t& error) const;
197 
201  ConstraintSetPtr_t pathConstraint (const EdgePtr_t& edge) const;
202 
204  void maxIterations (size_type iterations);
205 
207  size_type maxIterations () const;
208 
210  void errorThreshold (const value_type& threshold);
211 
213  value_type errorThreshold () const;
214 
216  const DevicePtr_t& robot () const;
217 
219  const ProblemPtr_t& problem () const;
220 
222  void problem (const ProblemPtr_t& problem);
223 
226  {
227  hists_.push_back (hist);
228  }
229 
231  const Histograms_t& histograms () const
232  {
233  return hists_;
234  }
235 
237  GraphComponentWkPtr_t get(std::size_t id) const;
238 
239  std::size_t nbComponents () const
240  {
241  return components_.size();
242  }
243 
245  virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
246 
248  virtual void initialize ();
249 
251  virtual void invalidate();
252 
253  protected:
255  void init (const GraphWkPtr_t& weak, DevicePtr_t robot);
256 
259  Graph (const std::string& name, const ProblemPtr_t& problem);
260 
262  std::ostream& print (std::ostream& os) const;
263 
264  private:
266  GraphComponents_t& components ();
267 
269  GraphComponents_t components_;
270 
272  StateSelectorPtr_t stateSelector_;
273 
276  ConstraintPtr_t constraints_;
277 
279  DevicePtr_t robot_;
280 
282  GraphWkPtr_t wkPtr_;
283 
285  typedef std::map < StatePtr_t, ConstraintSetPtr_t > MapFromState;
286  typedef std::pair < StatePtr_t, ConstraintSetPtr_t > PairStateConstraints;
287  MapFromState constraintSetMapFromState_;
288 
290  Histograms_t hists_;
291 
293  typedef std::map < EdgePtr_t, ConstraintSetPtr_t > MapFromEdge;
294  typedef std::pair < EdgePtr_t, ConstraintSetPtr_t > PairEdgeConstraints;
295  MapFromEdge cfgConstraintSetMapFromEdge_, pathConstraintSetMapFromEdge_;
296  ProblemPtr_t problem_;
297  value_type errorThreshold_;
298  size_type maxIterations_;
299 
300  ConstraintsAndComplements_t constraintsAndComplements_;
301  friend class GraphComponent;
302  }; // Class Graph
303 
305  } // namespace graph
306  } // namespace manipulation
307 
308 } // namespace hpp
309 
310 BOOST_CLASS_EXPORT_KEY(hpp::manipulation::graph::Graph)
311 
312 #endif // HPP_MANIPULATION_GRAPH_GRAPH_HH
StateHistogram NodeHistogram HPP_MANIPULATION_DEPRECATED
Definition: statistics.hh:200
const Histograms_t & histograms() const
Get the histograms.
Definition: graph.hh:231
std::vector< EdgePtr_t > Edges_t
Definition: fwd.hh:46
shared_ptr< StateSelector > StateSelectorPtr_t
Definition: fwd.hh:40
Definition: main.hh:1
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:38
core::vector_t vector_t
Definition: fwd.hh:85
shared_ptr< Histogram > HistogramPtr_t
Definition: fwd.hh:58
core::value_type value_type
Definition: fwd.hh:82
void insertHistogram(const graph::HistogramPtr_t &hist)
Register an histogram representing a foliation.
Definition: graph.hh:225
std::vector< ConstraintAndComplement_t > ConstraintsAndComplements_t
Definition: constraint-set.hh:82
#define HPP_MANIPULATION_DLLAPI
Definition: config.hh:64
core::ConstraintPtr_t ConstraintPtr_t
Definition: fwd.hh:105
std::list< HistogramPtr_t > Histograms_t
Definition: fwd.hh:62
std::size_t nbComponents() const
Definition: graph.hh:239
shared_ptr< Graph > GraphPtr_t
Definition: fwd.hh:35
StateSelectorPtr_t stateSelector() const
Get the state selector.
Definition: graph.hh:67
shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:55
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:115
constraints::ImplicitPtr_t ImplicitPtr_t
Definition: fwd.hh:108
shared_ptr< State > StatePtr_t
Definition: fwd.hh:36
core::size_type size_type
Definition: fwd.hh:83
shared_ptr< Device > DevicePtr_t
Definition: fwd.hh:29
shared_ptr< Edge > EdgePtr_t
Definition: fwd.hh:37
Define common methods of the graph components.
Definition: graph-component.hh:39
std::vector< GraphComponentWkPtr_t > GraphComponents_t
Definition: fwd.hh:44
Definition: graph.hh:45
Definition: roadmap-node.hh:32