hpp-manipulation  4.10.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 // 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 <boost/tuple/tuple.hpp>
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 
56  StateSelectorPtr_t createStateSelector (const std::string& name);
57 
62  void stateSelector (StateSelectorPtr_t ns);
63 
66  {
67  return stateSelector_;
68  }
69 
71  StatePtr_t getState (ConfigurationIn_t config) const;
72 
74  StatePtr_t getState (RoadmapNodePtr_t node) const;
75 
77  Edges_t getEdges (const StatePtr_t& from, const StatePtr_t& to) const;
78 
80  EdgePtr_t chooseEdge(RoadmapNodePtr_t node) const;
81 
95  void registerConstraints (const ImplicitPtr_t& constraint,
96  const ImplicitPtr_t& complement,
97  const ImplicitPtr_t& both);
98 
110  bool isComplement (const ImplicitPtr_t& constraint,
111  const ImplicitPtr_t& complement,
112  ImplicitPtr_t& combinationOfBoth) const;
113 
118  const ConstraintsAndComplements_t& constraintsAndComplements () const;
119 
123  ConstraintSetPtr_t configConstraint (const StatePtr_t& state) const;
124 
127  ConstraintSetPtr_t configConstraint (const EdgePtr_t& edge) const
129 
136  ConstraintSetPtr_t targetConstraint (const EdgePtr_t& edge) const;
137 
147  bool getConfigErrorForState (ConfigurationIn_t config,
148  const StatePtr_t& state, vector_t& error) const;
149 
157  // of the edge
161  bool getConfigErrorForEdge (ConfigurationIn_t config,
162  const EdgePtr_t& edge, vector_t& error) const;
163 
174  bool getConfigErrorForEdgeLeaf
175  (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
176  const EdgePtr_t& edge, vector_t& error) const;
177 
188  bool getConfigErrorForEdgeTarget
189  (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
190  const EdgePtr_t& edge, vector_t& error) const;
191 
195  ConstraintSetPtr_t pathConstraint (const EdgePtr_t& edge) const;
196 
198  void maxIterations (size_type iterations);
199 
201  size_type maxIterations () const;
202 
204  void errorThreshold (const value_type& threshold);
205 
207  value_type errorThreshold () const;
208 
210  const DevicePtr_t& robot () const;
211 
213  const ProblemPtr_t& problem () const;
214 
216  void problem (const ProblemPtr_t& problem);
217 
220  {
221  hists_.push_back (hist);
222  }
223 
225  const Histograms_t& histograms () const
226  {
227  return hists_;
228  }
229 
231  GraphComponentWkPtr_t get(std::size_t id) const;
232 
233  std::size_t nbComponents () const
234  {
235  return components_.size();
236  }
237 
239  virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
240 
242  virtual void initialize ();
243 
245  virtual void invalidate();
246 
247  protected:
249  void init (const GraphWkPtr_t& weak, DevicePtr_t robot);
250 
253  Graph (const std::string& name, const ProblemPtr_t& problem);
254 
256  std::ostream& print (std::ostream& os) const;
257 
258  private:
260  GraphComponents_t& components ();
261 
263  GraphComponents_t components_;
264 
266  StateSelectorPtr_t stateSelector_;
267 
270  ConstraintPtr_t constraints_;
271 
273  DevicePtr_t robot_;
274 
276  GraphWkPtr_t wkPtr_;
277 
279  typedef std::map < StatePtr_t, ConstraintSetPtr_t > MapFromState;
280  typedef std::pair < StatePtr_t, ConstraintSetPtr_t > PairStateConstraints;
281  MapFromState constraintSetMapFromState_;
282 
284  Histograms_t hists_;
285 
287  typedef std::map < EdgePtr_t, ConstraintSetPtr_t > MapFromEdge;
288  typedef std::pair < EdgePtr_t, ConstraintSetPtr_t > PairEdgeConstraints;
289  MapFromEdge cfgConstraintSetMapFromEdge_, pathConstraintSetMapFromEdge_;
290  ProblemPtr_t problem_;
291  value_type errorThreshold_;
292  size_type maxIterations_;
293 
294  ConstraintsAndComplements_t constraintsAndComplements_;
295  friend class GraphComponent;
296  }; // Class Graph
297 
299  } // namespace graph
300  } // namespace manipulation
301 
302 } // namespace hpp
303 
304 #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:225
std::vector< EdgePtr_t > Edges_t
Definition: fwd.hh:46
boost::shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:112
Definition: main.hh:1
boost::shared_ptr< StateSelector > StateSelectorPtr_t
Definition: fwd.hh:40
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:38
core::vector_t vector_t
Definition: fwd.hh:82
boost::shared_ptr< State > StatePtr_t
Definition: fwd.hh:36
core::value_type value_type
Definition: fwd.hh:79
void insertHistogram(const graph::HistogramPtr_t &hist)
Register an histogram representing a foliation.
Definition: graph.hh:219
std::vector< ConstraintAndComplement_t > ConstraintsAndComplements_t
Definition: constraint-set.hh:79
#define HPP_MANIPULATION_DLLAPI
Definition: config.hh:64
core::ConstraintPtr_t ConstraintPtr_t
Definition: fwd.hh:102
boost::shared_ptr< Histogram > HistogramPtr_t
Definition: fwd.hh:58
std::list< HistogramPtr_t > Histograms_t
Definition: fwd.hh:62
std::size_t nbComponents() const
Definition: graph.hh:233
boost::shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:55
boost::shared_ptr< Device > DevicePtr_t
Definition: fwd.hh:29
StateSelectorPtr_t stateSelector() const
Get the state selector.
Definition: graph.hh:65
constraints::ImplicitPtr_t ImplicitPtr_t
Definition: fwd.hh:105
boost::shared_ptr< Edge > EdgePtr_t
Definition: fwd.hh:37
core::size_type size_type
Definition: fwd.hh:80
Define common methods of the graph components.
Definition: graph-component.hh:39
boost::shared_ptr< Graph > GraphPtr_t
Definition: fwd.hh:35
std::vector< GraphComponentWkPtr_t > GraphComponents_t
Definition: fwd.hh:44
Definition: graph.hh:45
Definition: roadmap-node.hh:32