hpp-core  4.15.1
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 
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29 
30 #ifndef HPP_CORE_ROADMAP_HH
31 #define HPP_CORE_ROADMAP_HH
32 
33 #include <hpp/core/config.hh>
35 #include <hpp/core/fwd.hh>
36 #include <hpp/util/serialization-fwd.hh>
37 #include <iostream>
38 
39 namespace hpp {
40 namespace core {
43 
47  public:
49  static RoadmapPtr_t create(const DistancePtr_t& distance,
50  const DevicePtr_t& robot);
51 
53  virtual void clear();
54 
62 
64  return addNode(ConfigurationPtr_t(new Configuration_t(config)));
65  }
66 
72  NodePtr_t nearestNode(const Configuration_t& configuration,
73  value_type& minDistance, bool reverse = false);
74 
76  value_type& minDistance, bool reverse = false) {
77  return nearestNode(*configuration, minDistance, reverse);
78  }
79 
86  NodePtr_t nearestNode(const Configuration_t& configuration,
87  const ConnectedComponentPtr_t& connectedComponent,
88  value_type& minDistance, bool reverse = false);
89 
91  const ConnectedComponentPtr_t& connectedComponent,
92  value_type& minDistance, bool reverse = false) {
93  return nearestNode(*configuration, connectedComponent, minDistance,
94  reverse);
95  }
96 
102  Nodes_t nearestNodes(const Configuration_t& configuration, size_type k);
103 
105  return nearestNodes(*configuration, k);
106  }
107 
114  Nodes_t nearestNodes(const Configuration_t& configuration,
115  const ConnectedComponentPtr_t& connectedComponent,
116  size_type k);
117 
119  const ConnectedComponentPtr_t& connectedComponent,
120  size_type k) {
121  return nearestNodes(*configuration, connectedComponent, k);
122  }
123 
127  const Configuration_t& configuration,
128  const ConnectedComponentPtr_t& connectedComponent,
129  value_type maxDistance);
130 
141  const PathPtr_t path);
142 
153  const PathPtr_t path);
154 
165  const PathPtr_t path);
166 
168  EdgePtr_t addEdge(const NodePtr_t& n1, const NodePtr_t& n2,
169  const PathPtr_t& path);
170 
176  void addEdges(const NodePtr_t from, const NodePtr_t& to,
177  const PathPtr_t& path);
178 
180  void merge(const RoadmapPtr_t& other);
181 
186  void insertPathVector(const PathVectorPtr_t& path, bool backAndForth);
187 
193 
194  void resetGoalNodes() { goalNodes_.clear(); }
195 
196  void initNode(const ConfigurationPtr_t& config) {
197  initNode_ = addNode(config);
198  }
199 
200  virtual ~Roadmap();
202  bool pathExists() const;
203  const Nodes_t& nodes() const { return nodes_; }
204  const Edges_t& edges() const { return edges_; }
205  NodePtr_t initNode() const { return initNode_; }
206  const NodeVector_t& goalNodes() const { return goalNodes_; }
209 
212 
214  void nearestNeighbor(NearestNeighborPtr_t nearestNeighbor);
215 
219  const DistancePtr_t& distance() const;
222  std::ostream& print(std::ostream& os) const;
223 
224  protected:
227  Roadmap(const DistancePtr_t& distance, const DevicePtr_t& robot);
228 
229  Roadmap(){};
230 
234  void addConnectedComponent(const NodePtr_t& node);
235 
238  virtual void push_node(const NodePtr_t& n) { nodes_.push_back(n); }
239 
249  virtual void impl_addEdge(const EdgePtr_t& e);
250 
254  virtual NodePtr_t createNode(const ConfigurationPtr_t& configuration) const;
255 
257  void init(RoadmapWkPtr_t weak);
258 
259  private:
268  NodePtr_t addNode(const ConfigurationPtr_t& config,
269  ConnectedComponentPtr_t connectedComponent);
270 
274  void connect(const ConnectedComponentPtr_t& cc1,
275  const ConnectedComponentPtr_t& cc2);
276 
280  void merge(const ConnectedComponentPtr_t& cc1,
282 
283  const DistancePtr_t distance_;
284  ConnectedComponents_t connectedComponents_;
285  Nodes_t nodes_;
286  Edges_t edges_;
287  NodePtr_t initNode_;
288  NodeVector_t goalNodes_;
289  NearestNeighborPtr_t nearestNeighbor_;
290  RoadmapWkPtr_t weak_;
291 
292  HPP_SERIALIZABLE();
293 }; // class Roadmap
294 std::ostream& operator<<(std::ostream& os, const Roadmap& r);
296 } // namespace core
297 } // namespace hpp
298 #endif // HPP_CORE_ROADMAP_HH
std::set< RawPtr_t > RawPtrs_t
Definition: connected-component.hh:46
Definition: edge.hh:46
Optimization of the nearest neighbor search.
Definition: nearest-neighbor.hh:39
Definition: node.hh:46
Definition: roadmap.hh:46
std::ostream & print(std::ostream &os) const
void insertPathVector(const PathVectorPtr_t &path, bool backAndForth)
NodePtr_t initNode() const
Definition: roadmap.hh:205
NodePtr_t nearestNode(const ConfigurationPtr_t &configuration, const ConnectedComponentPtr_t &connectedComponent, value_type &minDistance, bool reverse=false)
Definition: roadmap.hh:90
NodePtr_t addNodeAndEdge(const ConfigurationPtr_t &from, const NodePtr_t to, const PathPtr_t path)
void init(RoadmapWkPtr_t weak)
Store weak pointer to itself.
bool pathExists() const
Check that a path exists between the initial node and one goal node.
void addEdges(const NodePtr_t from, const NodePtr_t &to, const PathPtr_t &path)
EdgePtr_t addEdge(const NodePtr_t &n1, const NodePtr_t &n2, const PathPtr_t &path)
Add an edge between two nodes.
virtual NodePtr_t createNode(const ConfigurationPtr_t &configuration) const
void initNode(const ConfigurationPtr_t &config)
Definition: roadmap.hh:196
const Nodes_t & nodes() const
Definition: roadmap.hh:203
void addConnectedComponent(const NodePtr_t &node)
void resetGoalNodes()
Definition: roadmap.hh:194
const ConnectedComponents_t & connectedComponents() const
Get list of connected component of the roadmap.
NodePtr_t nearestNode(const Configuration_t &configuration, const ConnectedComponentPtr_t &connectedComponent, value_type &minDistance, bool reverse=false)
virtual void push_node(const NodePtr_t &n)
Definition: roadmap.hh:238
virtual void impl_addEdge(const EdgePtr_t &e)
Nodes_t nearestNodes(const Configuration_t &configuration, size_type k)
void merge(const RoadmapPtr_t &other)
Add the nodes and edges of a roadmap into this one.
NodePtr_t addGoalNode(const ConfigurationPtr_t &config)
Nodes_t nearestNodes(const Configuration_t &configuration, const ConnectedComponentPtr_t &connectedComponent, size_type k)
NearestNeighborPtr_t nearestNeighbor()
Get nearestNeighbor object.
const Edges_t & edges() const
Definition: roadmap.hh:204
NodePtr_t nearestNode(const ConfigurationPtr_t &configuration, value_type &minDistance, bool reverse=false)
Definition: roadmap.hh:75
NodePtr_t addNodeAndEdge(const NodePtr_t from, const ConfigurationPtr_t &to, const PathPtr_t path)
const NodeVector_t & goalNodes() const
Definition: roadmap.hh:206
void nearestNeighbor(NearestNeighborPtr_t nearestNeighbor)
Set new NearestNeighbor (roadmap must be empty)
virtual void clear()
Clear the roadmap by deleting nodes and edges.
Roadmap()
Definition: roadmap.hh:229
NodePtr_t addNodeAndEdges(const NodePtr_t from, const ConfigurationPtr_t &to, const PathPtr_t path)
static RoadmapPtr_t create(const DistancePtr_t &distance, const DevicePtr_t &robot)
Return shared pointer to new instance.
NodePtr_t addNode(const Configuration_t &config)
Definition: roadmap.hh:63
const DistancePtr_t & distance() const
NodeVector_t nodesWithinBall(const Configuration_t &configuration, const ConnectedComponentPtr_t &connectedComponent, value_type maxDistance)
NodePtr_t addNode(const ConfigurationPtr_t &config)
Nodes_t nearestNodes(const ConfigurationPtr_t &configuration, const ConnectedComponentPtr_t &connectedComponent, size_type k)
Definition: roadmap.hh:118
Roadmap(const DistancePtr_t &distance, const DevicePtr_t &robot)
NodePtr_t nearestNode(const Configuration_t &configuration, value_type &minDistance, bool reverse=false)
Nodes_t nearestNodes(const ConfigurationPtr_t &configuration, size_type k)
Definition: roadmap.hh:104
#define HPP_CORE_DLLAPI
Definition: config.hh:64
std::ostream & operator<<(std::ostream &os, const Constraint &constraint)
Definition: constraint.hh:99
pinocchio::value_type value_type
Definition: fwd.hh:174
shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:193
shared_ptr< Distance > DistancePtr_t
Definition: fwd.hh:141
std::vector< NodePtr_t > NodeVector_t
Definition: fwd.hh:182
std::set< ConnectedComponentPtr_t, SharedComparator > ConnectedComponents_t
Definition: fwd.hh:127
std::list< Edge * > Edges_t
Definition: fwd.hh:145
shared_ptr< Roadmap > RoadmapPtr_t
Definition: fwd.hh:199
pinocchio::size_type size_type
Definition: fwd.hh:173
std::list< NodePtr_t > Nodes_t
Definition: fwd.hh:181
pinocchio::ConfigurationPtr_t ConfigurationPtr_t
Definition: fwd.hh:109
shared_ptr< ConnectedComponent > ConnectedComponentPtr_t
Definition: fwd.hh:117
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:106
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:134
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:187
Definition: bi-rrt-planner.hh:35