hpp-core 4.14.0
Implement basic classes for canonical path planning for kinematic chains.
Loading...
Searching...
No Matches
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
39namespace hpp {
40namespace 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
73 value_type& minDistance, bool reverse = false);
74
76 value_type& minDistance, bool reverse = false) {
77 return nearestNode(*configuration, minDistance, reverse);
78 }
79
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
103
105 return nearestNodes(*configuration, k);
106 }
107
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
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
230
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
294std::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
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)
const Edges_t & edges() const
Definition: roadmap.hh:204
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.
const ConnectedComponents_t & connectedComponents() const
Get list of connected component of the roadmap.
virtual NodePtr_t createNode(const ConfigurationPtr_t &configuration) const
void initNode(const ConfigurationPtr_t &config)
Definition: roadmap.hh:196
void addConnectedComponent(const NodePtr_t &node)
void resetGoalNodes()
Definition: roadmap.hh:194
const NodeVector_t & goalNodes() const
Definition: roadmap.hh:206
const DistancePtr_t & distance() const
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
std::ostream & print(std::ostream &os) const
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 Nodes_t & nodes() const
Definition: roadmap.hh:203
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)
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
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:173
shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:192
shared_ptr< Distance > DistancePtr_t
Definition: fwd.hh:140
std::vector< NodePtr_t > NodeVector_t
Definition: fwd.hh:181
std::set< ConnectedComponentPtr_t, SharedComparator > ConnectedComponents_t
Definition: fwd.hh:126
std::list< Edge * > Edges_t
Definition: fwd.hh:144
shared_ptr< Roadmap > RoadmapPtr_t
Definition: fwd.hh:198
pinocchio::size_type size_type
Definition: fwd.hh:172
std::list< NodePtr_t > Nodes_t
Definition: fwd.hh:180
pinocchio::ConfigurationPtr_t ConfigurationPtr_t
Definition: fwd.hh:108
shared_ptr< ConnectedComponent > ConnectedComponentPtr_t
Definition: fwd.hh:116
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:105
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:133
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:186
Definition: bi-rrt-planner.hh:35