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 
65 
68  {
69  return stateSelector_;
70  }
71 
74 
77 
79  Edges_t getEdges (const StatePtr_t& from, const StatePtr_t& to) const;
80 
83 
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 
125 
130 
135 
143 
154  const StatePtr_t& state, vector_t& error) const;
155 
163  // of the edge
168  const EdgePtr_t& edge, vector_t& error) const;
169 
181  (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
182  const EdgePtr_t& edge, vector_t& error) const;
183 
195  (ConfigurationIn_t leafConfig, ConfigurationIn_t config,
196  const EdgePtr_t& edge, vector_t& error) const;
197 
202 
204  void maxIterations (size_type iterations);
205 
208 
210  void errorThreshold (const value_type& threshold);
211 
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
Definition: roadmap-node.hh:33
Define common methods of the graph components.
Definition: graph-component.hh:40
Definition: graph.hh:46
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:225
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:67
const Histograms_t & histograms() const
Get the histograms.
Definition: graph.hh:231
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:239
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:82
std::vector< EdgePtr_t > Edges_t
Definition: fwd.hh:46
shared_ptr< StateSelector > StateSelectorPtr_t
Definition: fwd.hh:40
shared_ptr< Graph > GraphPtr_t
Definition: fwd.hh:35
shared_ptr< State > StatePtr_t
Definition: fwd.hh:36
shared_ptr< Edge > EdgePtr_t
Definition: fwd.hh:37
StateHistogram NodeHistogram HPP_MANIPULATION_DEPRECATED
Definition: statistics.hh:200
std::list< HistogramPtr_t > Histograms_t
Definition: fwd.hh:62
shared_ptr< Histogram > HistogramPtr_t
Definition: fwd.hh:58
std::vector< GraphComponentWkPtr_t > GraphComponents_t
Definition: fwd.hh:44
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
core::size_type size_type
Definition: fwd.hh:83
core::value_type value_type
Definition: fwd.hh:82
shared_ptr< Device > DevicePtr_t
Definition: fwd.hh:29
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:38
core::vector_t vector_t
Definition: fwd.hh:85
core::ConstraintPtr_t ConstraintPtr_t
Definition: fwd.hh:105
Definition: main.hh:1