hpp-manipulation  4.11.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 
97  void registerConstraints (const ImplicitPtr_t& constraint,
98  const ImplicitPtr_t& complement,
99  const ImplicitPtr_t& both);
100 
112  bool isComplement (const ImplicitPtr_t& constraint,
113  const ImplicitPtr_t& complement,
114  ImplicitPtr_t& combinationOfBoth) const;
115 
120  const ConstraintsAndComplements_t& constraintsAndComplements () const;
121 
125  ConstraintSetPtr_t configConstraint (const StatePtr_t& state) const;
126 
129  ConstraintSetPtr_t configConstraint (const EdgePtr_t& edge) const
131 
138  ConstraintSetPtr_t targetConstraint (const EdgePtr_t& edge) const;
139 
149  bool getConfigErrorForState (ConfigurationIn_t config,
150  const StatePtr_t& state, vector_t& error) const;
151 
159  // of the edge
163  bool getConfigErrorForEdge (ConfigurationIn_t config,
164  const EdgePtr_t& edge, vector_t& error) const;
165 
176  bool getConfigErrorForEdgeLeaf
177  (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
178  const EdgePtr_t& edge, vector_t& error) const;
179 
190  bool getConfigErrorForEdgeTarget
191  (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
192  const EdgePtr_t& edge, vector_t& error) const;
193 
197  ConstraintSetPtr_t pathConstraint (const EdgePtr_t& edge) const;
198 
200  void maxIterations (size_type iterations);
201 
203  size_type maxIterations () const;
204 
206  void errorThreshold (const value_type& threshold);
207 
209  value_type errorThreshold () const;
210 
212  const DevicePtr_t& robot () const;
213 
215  const ProblemPtr_t& problem () const;
216 
218  void problem (const ProblemPtr_t& problem);
219 
222  {
223  hists_.push_back (hist);
224  }
225 
227  const Histograms_t& histograms () const
228  {
229  return hists_;
230  }
231 
233  GraphComponentWkPtr_t get(std::size_t id) const;
234 
235  std::size_t nbComponents () const
236  {
237  return components_.size();
238  }
239 
241  virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
242 
244  virtual void initialize ();
245 
247  virtual void invalidate();
248 
249  protected:
251  void init (const GraphWkPtr_t& weak, DevicePtr_t robot);
252 
255  Graph (const std::string& name, const ProblemPtr_t& problem);
256 
258  std::ostream& print (std::ostream& os) const;
259 
260  private:
262  GraphComponents_t& components ();
263 
265  GraphComponents_t components_;
266 
268  StateSelectorPtr_t stateSelector_;
269 
272  ConstraintPtr_t constraints_;
273 
275  DevicePtr_t robot_;
276 
278  GraphWkPtr_t wkPtr_;
279 
281  typedef std::map < StatePtr_t, ConstraintSetPtr_t > MapFromState;
282  typedef std::pair < StatePtr_t, ConstraintSetPtr_t > PairStateConstraints;
283  MapFromState constraintSetMapFromState_;
284 
286  Histograms_t hists_;
287 
289  typedef std::map < EdgePtr_t, ConstraintSetPtr_t > MapFromEdge;
290  typedef std::pair < EdgePtr_t, ConstraintSetPtr_t > PairEdgeConstraints;
291  MapFromEdge cfgConstraintSetMapFromEdge_, pathConstraintSetMapFromEdge_;
292  ProblemPtr_t problem_;
293  value_type errorThreshold_;
294  size_type maxIterations_;
295 
296  ConstraintsAndComplements_t constraintsAndComplements_;
297  friend class GraphComponent;
298  }; // Class Graph
299 
301  } // namespace graph
302  } // namespace manipulation
303 
304 } // namespace hpp
305 
306 BOOST_CLASS_EXPORT_KEY(hpp::manipulation::graph::Graph)
307 
308 #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:227
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:221
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:235
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