hpp-core  4.11.0
Implement basic classes for canonical path planning for kinematic chains.
roadmap.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2014 CNRS
3 // Authors: Florent Lamiraux
4 //
5 // This file is part of hpp-core
6 // hpp-core is free software: you can redistribute it
7 // and/or modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation, either version
9 // 3 of the License, or (at your option) any later version.
10 //
11 // hpp-core is distributed in the hope that it will be
12 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Lesser Public License for more details. You should have
15 // received a copy of the GNU Lesser General Public License along with
16 // hpp-core If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef HPP_CORE_ROADMAP_HH
20 # define HPP_CORE_ROADMAP_HH
21 
22 # include <iostream>
23 
24 # include <hpp/core/fwd.hh>
25 # include <hpp/core/config.hh>
26 
28 # include <hpp/util/serialization-fwd.hh>
29 
30 namespace hpp {
31  namespace core {
34 
38  public:
40  static RoadmapPtr_t create (const DistancePtr_t& distance, const DevicePtr_t& robot);
41 
43  virtual void clear ();
44 
51  NodePtr_t addNode (const ConfigurationPtr_t& config);
52 
54  {
55  return addNode (ConfigurationPtr_t (new Configuration_t(config)));
56  }
57 
63  NodePtr_t nearestNode (const Configuration_t& configuration,
64  value_type& minDistance, bool reverse = false);
65 
66  NodePtr_t nearestNode (const ConfigurationPtr_t& configuration,
67  value_type& minDistance, bool reverse = false)
68  {
69  return nearestNode (*configuration, minDistance, reverse);
70  }
71 
78  NodePtr_t nearestNode (const Configuration_t& configuration,
79  const ConnectedComponentPtr_t& connectedComponent,
80  value_type& minDistance, bool reverse = false);
81 
82  NodePtr_t nearestNode (const ConfigurationPtr_t& configuration,
83  const ConnectedComponentPtr_t& connectedComponent,
84  value_type& minDistance, bool reverse = false)
85  {
86  return nearestNode (*configuration, connectedComponent, minDistance, reverse);
87  }
88 
94  Nodes_t nearestNodes (const Configuration_t& configuration,
95  size_type k);
96 
97  Nodes_t nearestNodes (const ConfigurationPtr_t& configuration,
98  size_type k)
99  {
100  return nearestNodes (*configuration, k);
101  }
102 
109  Nodes_t nearestNodes (const Configuration_t& configuration,
111  connectedComponent,
112  size_type k);
113 
114  Nodes_t nearestNodes (const ConfigurationPtr_t& configuration,
116  connectedComponent,
117  size_type k)
118  {
119  return nearestNodes (*configuration, connectedComponent, k);
120  }
121 
124  NodeVector_t nodesWithinBall (const Configuration_t& configuration,
125  const ConnectedComponentPtr_t& connectedComponent,
126  value_type maxDistance);
127 
137  NodePtr_t addNodeAndEdges (const NodePtr_t from,
138  const ConfigurationPtr_t& to,
139  const PathPtr_t path);
140 
150  NodePtr_t addNodeAndEdge (const NodePtr_t from,
151  const ConfigurationPtr_t& to,
152  const PathPtr_t path);
153 
163  NodePtr_t addNodeAndEdge (const ConfigurationPtr_t& from,
164  const NodePtr_t to,
165  const PathPtr_t path);
166 
167 
169  EdgePtr_t addEdge (const NodePtr_t& n1, const NodePtr_t& n2,
170  const PathPtr_t& path);
171 
177  void addEdges (const NodePtr_t from, const NodePtr_t& to,
178  const PathPtr_t& path);
179 
184  NodePtr_t addGoalNode (const ConfigurationPtr_t& config);
185 
187  {
188  goalNodes_.clear ();
189  }
190 
191  void initNode (const ConfigurationPtr_t& config)
192  {
193  initNode_ = addNode (config);
194  }
195 
196  virtual ~Roadmap ();
198  bool pathExists () const;
199  const Nodes_t& nodes () const
200  {
201  return nodes_;
202  }
203  const Edges_t& edges () const
204  {
205  return edges_;
206  }
208  {
209  return initNode_;
210  }
211  const NodeVector_t& goalNodes () const
212  {
213  return goalNodes_;
214  }
216  const ConnectedComponents_t& connectedComponents () const;
217 
219  NearestNeighborPtr_t nearestNeighbor();
220 
222  void nearestNeighbor(NearestNeighborPtr_t nearestNeighbor);
223 
227  const DistancePtr_t& distance () const;
230  std::ostream& print (std::ostream& os) const;
231 
232  protected:
235  Roadmap (const DistancePtr_t& distance, const DevicePtr_t& robot);
236 
237  Roadmap () {};
238 
242  void addConnectedComponent (const NodePtr_t& node);
243 
246  virtual void push_node (const NodePtr_t& n)
247  {
248  nodes_.push_back (n);
249  }
250 
260  virtual void impl_addEdge (const EdgePtr_t& e);
261 
265  virtual NodePtr_t createNode (const ConfigurationPtr_t& configuration) const;
266 
268  void init (RoadmapWkPtr_t weak);
269 
270  private:
279  NodePtr_t addNode (const ConfigurationPtr_t& config,
280  ConnectedComponentPtr_t connectedComponent);
281 
285  void connect (const ConnectedComponentPtr_t& cc1,
286  const ConnectedComponentPtr_t& cc2);
287 
291  void merge (const ConnectedComponentPtr_t& cc1,
293 
294  const DistancePtr_t distance_;
295  ConnectedComponents_t connectedComponents_;
296  Nodes_t nodes_;
297  Edges_t edges_;
298  NodePtr_t initNode_;
299  NodeVector_t goalNodes_;
300  NearestNeighborPtr_t nearestNeighbor_;
301  RoadmapWkPtr_t weak_;
302 
303  HPP_SERIALIZABLE();
304  }; // class Roadmap
305  std::ostream& operator<< (std::ostream& os, const Roadmap& r);
307  } // namespace core
308 } // namespace hpp
309 #endif // HPP_CORE_ROADMAP_HH
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:114
Definition: bi-rrt-planner.hh:24
Definition: edge.hh:35
pinocchio::size_type size_type
Definition: fwd.hh:156
virtual void push_node(const NodePtr_t &n)
Definition: roadmap.hh:246
Optimization of the nearest neighbor search.
Definition: nearest-neighbor.hh:28
shared_ptr< Distance > DistancePtr_t
Definition: fwd.hh:122
NodePtr_t addNode(const Configuration_t &config)
Definition: roadmap.hh:53
std::set< ConnectedComponentPtr_t > ConnectedComponents_t
Definition: fwd.hh:108
Definition: node.hh:35
shared_ptr< ConnectedComponent > ConnectedComponentPtr_t
Definition: fwd.hh:107
std::list< NodePtr_t > Nodes_t
Definition: fwd.hh:164
pinocchio::value_type value_type
Definition: fwd.hh:157
std::set< RawPtr_t > RawPtrs_t
Definition: connected-component.hh:35
Roadmap()
Definition: roadmap.hh:237
Nodes_t nearestNodes(const ConfigurationPtr_t &configuration, size_type k)
Definition: roadmap.hh:97
shared_ptr< Roadmap > RoadmapPtr_t
Definition: fwd.hh:182
const Nodes_t & nodes() const
Definition: roadmap.hh:199
Nodes_t nearestNodes(const ConfigurationPtr_t &configuration, const ConnectedComponentPtr_t &connectedComponent, size_type k)
Definition: roadmap.hh:114
pinocchio::ConfigurationPtr_t ConfigurationPtr_t
Definition: fwd.hh:99
std::vector< NodePtr_t > NodeVector_t
Definition: fwd.hh:165
std::list< Edge * > Edges_t
Definition: fwd.hh:127
void initNode(const ConfigurationPtr_t &config)
Definition: roadmap.hh:191
void resetGoalNodes()
Definition: roadmap.hh:186
NodePtr_t nearestNode(const ConfigurationPtr_t &configuration, const ConnectedComponentPtr_t &connectedComponent, value_type &minDistance, bool reverse=false)
Definition: roadmap.hh:82
NodePtr_t nearestNode(const ConfigurationPtr_t &configuration, value_type &minDistance, bool reverse=false)
Definition: roadmap.hh:66
const Edges_t & edges() const
Definition: roadmap.hh:203
std::ostream & operator<<(std::ostream &os, const Constraint &constraint)
Definition: constraint.hh:101
#define HPP_CORE_DLLAPI
Definition: config.hh:64
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:96
const NodeVector_t & goalNodes() const
Definition: roadmap.hh:211
Definition: roadmap.hh:37
NodePtr_t initNode() const
Definition: roadmap.hh:207
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:170