hpp-core  4.12.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 
181  void merge(const RoadmapPtr_t& other);
182 
187  void insertPathVector(const PathVectorPtr_t& path, bool backAndForth);
188 
193  NodePtr_t addGoalNode (const ConfigurationPtr_t& config);
194 
196  {
197  goalNodes_.clear ();
198  }
199 
200  void initNode (const ConfigurationPtr_t& config)
201  {
202  initNode_ = addNode (config);
203  }
204 
205  virtual ~Roadmap ();
207  bool pathExists () const;
208  const Nodes_t& nodes () const
209  {
210  return nodes_;
211  }
212  const Edges_t& edges () const
213  {
214  return edges_;
215  }
217  {
218  return initNode_;
219  }
220  const NodeVector_t& goalNodes () const
221  {
222  return goalNodes_;
223  }
225  const ConnectedComponents_t& connectedComponents () const;
226 
228  NearestNeighborPtr_t nearestNeighbor();
229 
231  void nearestNeighbor(NearestNeighborPtr_t nearestNeighbor);
232 
236  const DistancePtr_t& distance () const;
239  std::ostream& print (std::ostream& os) const;
240 
241  protected:
244  Roadmap (const DistancePtr_t& distance, const DevicePtr_t& robot);
245 
246  Roadmap () {};
247 
251  void addConnectedComponent (const NodePtr_t& node);
252 
255  virtual void push_node (const NodePtr_t& n)
256  {
257  nodes_.push_back (n);
258  }
259 
269  virtual void impl_addEdge (const EdgePtr_t& e);
270 
274  virtual NodePtr_t createNode (const ConfigurationPtr_t& configuration) const;
275 
277  void init (RoadmapWkPtr_t weak);
278 
279  private:
288  NodePtr_t addNode (const ConfigurationPtr_t& config,
289  ConnectedComponentPtr_t connectedComponent);
290 
294  void connect (const ConnectedComponentPtr_t& cc1,
295  const ConnectedComponentPtr_t& cc2);
296 
300  void merge (const ConnectedComponentPtr_t& cc1,
302 
303  const DistancePtr_t distance_;
304  ConnectedComponents_t connectedComponents_;
305  Nodes_t nodes_;
306  Edges_t edges_;
307  NodePtr_t initNode_;
308  NodeVector_t goalNodes_;
309  NearestNeighborPtr_t nearestNeighbor_;
310  RoadmapWkPtr_t weak_;
311 
312  HPP_SERIALIZABLE();
313  }; // class Roadmap
314  std::ostream& operator<< (std::ostream& os, const Roadmap& r);
316  } // namespace core
317 } // namespace hpp
318 #endif // HPP_CORE_ROADMAP_HH
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:114
shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:176
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:255
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:246
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:208
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:200
void resetGoalNodes()
Definition: roadmap.hh:195
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:212
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:220
Definition: roadmap.hh:37
NodePtr_t initNode() const
Definition: roadmap.hh:216
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:170