hpp-manipulation  4.14.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 
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 
77 
79  StateSelectorPtr_t stateSelector() const { return stateSelector_; }
80 
83 
86 
88  Edges_t getEdges(const StatePtr_t& from, const StatePtr_t& to) const;
89 
92 
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 
134 
139 
144 
152 
163  vector_t& error) const;
164 
172  // of the edge
177  vector_t& error) const;
178 
190  ConfigurationIn_t config,
191  const EdgePtr_t& edge, vector_t& error) const;
192 
204  ConfigurationIn_t config,
205  const EdgePtr_t& edge,
206  vector_t& error) const;
207 
212 
214  void maxIterations(size_type iterations);
215 
218 
220  void errorThreshold(const value_type& threshold);
221 
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
Definition: roadmap-node.hh:42
Define common methods of the graph components.
Definition: graph-component.hh:51
Definition: graph.hh:58
void stateSelector(StateSelectorPtr_t ns)
StateSelectorPtr_t createStateSelector(const std::string &name)
Create and insert a state selector inside the graph.
value_type errorThreshold() const
Get error threshold in config projector.
void errorThreshold(const value_type &threshold)
Set error threshold.
virtual void initialize()
Initialize all components of the graph (edges and states)
bool getConfigErrorForEdge(ConfigurationIn_t config, const EdgePtr_t &edge, vector_t &error) const
const DevicePtr_t & robot() const
Get the robot.
void problem(const ProblemPtr_t &problem)
Set the problem.
Edges_t getEdges(const StatePtr_t &from, const StatePtr_t &to) const
Get possible edges between two nodes.
GraphComponentWkPtr_t get(std::size_t id) const
Get the component by its ID.
const ConstraintsAndComplements_t & constraintsAndComplements() const
void insertHistogram(const graph::HistogramPtr_t &hist)
Register an histogram representing a foliation.
Definition: graph.hh:235
ConstraintSetPtr_t configConstraint(const StatePtr_t &state) const
StatePtr_t getState(RoadmapNodePtr_t node) const
Returns the state of a roadmap node.
static GraphPtr_t create(const std::string &name, DevicePtr_t robot, const ProblemPtr_t &problem)
Graph(const std::string &name, const ProblemPtr_t &problem)
void maxIterations(size_type iterations)
Set maximal number of iterations.
void registerConstraints(const ImplicitPtr_t &constraint, const ImplicitPtr_t &complement, const ImplicitPtr_t &both)
size_type maxIterations() const
Get maximal number of iterations in config projector.
ConstraintSetPtr_t targetConstraint(const EdgePtr_t &edge) const
virtual void invalidate()
Invalidate all states and edges of the graph.
StateSelectorPtr_t stateSelector() const
Get the state selector.
Definition: graph.hh:79
const Histograms_t & histograms() const
Get the histograms.
Definition: graph.hh:240
bool getConfigErrorForEdgeLeaf(ConfigurationIn_t leafConfig, ConfigurationIn_t config, const EdgePtr_t &edge, vector_t &error) const
std::ostream & print(std::ostream &os) const
Print the object in a stream.
bool getConfigErrorForState(ConfigurationIn_t config, const StatePtr_t &state, vector_t &error) const
const ProblemPtr_t & problem() const
Get the problem.
std::size_t nbComponents() const
Definition: graph.hh:245
virtual std::ostream & dotPrint(std::ostream &os, dot::DrawingAttributes da=dot::DrawingAttributes()) const
Print the component in DOT language.
ConstraintSetPtr_t configConstraint(const EdgePtr_t &edge) const HPP_MANIPULATION_DEPRECATED
void init(const GraphWkPtr_t &weak, DevicePtr_t robot)
Initialization of the object.
EdgePtr_t chooseEdge(RoadmapNodePtr_t node) const
Select randomly outgoing edge of the given node.
ConstraintSetPtr_t pathConstraint(const EdgePtr_t &edge) const
bool isComplement(const ImplicitPtr_t &constraint, const ImplicitPtr_t &complement, ImplicitPtr_t &combinationOfBoth) const
StatePtr_t getState(ConfigurationIn_t config) const
Returns the state of a configuration.
bool getConfigErrorForEdgeTarget(ConfigurationIn_t leafConfig, ConfigurationIn_t config, const EdgePtr_t &edge, vector_t &error) const
#define HPP_MANIPULATION_DLLAPI
Definition: config.hh:64
std::vector< ConstraintAndComplement_t > ConstraintsAndComplements_t
Definition: constraint-set.hh:87
shared_ptr< Edge > EdgePtr_t
Definition: fwd.hh:49
shared_ptr< State > StatePtr_t
Definition: fwd.hh:48
std::vector< EdgePtr_t > Edges_t
Definition: fwd.hh:57
StateHistogram NodeHistogram HPP_MANIPULATION_DEPRECATED
Definition: statistics.hh:206
shared_ptr< StateSelector > StateSelectorPtr_t
Definition: fwd.hh:52
std::list< HistogramPtr_t > Histograms_t
Definition: fwd.hh:73
shared_ptr< Histogram > HistogramPtr_t
Definition: fwd.hh:69
std::vector< GraphComponentWkPtr_t > GraphComponents_t
Definition: fwd.hh:55
shared_ptr< Graph > GraphPtr_t
Definition: fwd.hh:47
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
core::value_type value_type
Definition: fwd.hh:90
shared_ptr< Device > DevicePtr_t
Definition: fwd.hh:40
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:49
core::vector_t vector_t
Definition: fwd.hh:93
core::ConstraintPtr_t ConstraintPtr_t
Definition: fwd.hh:113
Definition: main.hh:1