hpp-core: include/hpp/core/roadmap.hh Source File

hpp-core 9.0.2 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>
34#include <hpp/core/connected-component.hh>
35#include <hpp/core/fwd.hh>
36#include <hpp/util/serialization-fwd.hh>
37#include <iostream>
38
39namespace hpp {
40namespace core {
43
46 class HPP_CORE_DLLAPI Roadmap {
47 public:
49 static RoadmapPtr_t create(const DistancePtr_t& distance,
50 const DevicePtr_t& robot);
51
53 virtual void clear();
54
61 NodePtr_t addNode(ConfigurationIn_t config);
62
68 NodePtr_t nearestNode(ConfigurationIn_t configuration,
69 value_type& minDistance, bool reverse = false);
70
77 NodePtr_t nearestNode(ConfigurationIn_t configuration,
78 const ConnectedComponentPtr_t& connectedComponent,
79 value_type& minDistance, bool reverse = false);
80
86 Nodes_t nearestNodes(ConfigurationIn_t configuration, size_type k);
87
94 Nodes_t nearestNodes(ConfigurationIn_t configuration,
95 const ConnectedComponentPtr_t& connectedComponent,
96 size_type k);
97
100 NodeVector_t nodesWithinBall(
101 ConfigurationIn_t configuration,
102 const ConnectedComponentPtr_t& connectedComponent,
103 value_type maxDistance);
104
114 NodePtr_t addNodeAndEdges(const NodePtr_t from, ConfigurationIn_t to,
115 const PathPtr_t path);
116
126 NodePtr_t addNodeAndEdge(const NodePtr_t from, ConfigurationIn_t to,
127 const PathPtr_t path);
128
138 NodePtr_t addNodeAndEdge(ConfigurationIn_t from, const NodePtr_t to,
139 const PathPtr_t path);
140
142 EdgePtr_t addEdge(const NodePtr_t& n1, const NodePtr_t& n2,
143 const PathPtr_t& path);
144
150 void addEdges(const NodePtr_t from, const NodePtr_t& to,
151 const PathPtr_t& path);
152
154 void merge(const RoadmapPtr_t& other);
155
160 void insertPathVector(const PathVectorPtr_t& path, bool backAndForth);
161
166 NodePtr_t addGoalNode(ConfigurationIn_t config);
167
168 void resetGoalNodes() { goalNodes_.clear(); }
169
170 void initNode(ConfigurationIn_t config) { initNode_ = addNode(config); }
171
172 virtual ~Roadmap();
174 bool pathExists() const;
175 const Nodes_t& nodes() const { return nodes_; }
176 const Edges_t& edges() const { return edges_; }
177 NodePtr_t initNode() const { return initNode_; }
178 const NodeVector_t& goalNodes() const { return goalNodes_; }
180 const ConnectedComponents_t& connectedComponents() const;
181
183 NearestNeighborPtr_t nearestNeighbor();
184
186 void nearestNeighbor(NearestNeighborPtr_t nearestNeighbor);
187
191 const DistancePtr_t& distance() const;
193 const path::CostPtr_t cost() const;
195 void cost(const path::CostPtr_t& cost);
198 std::ostream& print(std::ostream& os) const;
199
200 protected:
203 Roadmap(const DistancePtr_t& distance, const DevicePtr_t& robot);
204
205 Roadmap() {};
206
210 void addConnectedComponent(const NodePtr_t& node);
211
214 virtual void push_node(const NodePtr_t& n) { nodes_.push_back(n); }
215
225 virtual void impl_addEdge(const EdgePtr_t& e);
226
230 virtual NodePtr_t createNode(ConfigurationIn_t configuration) const;
231
233 void init(RoadmapWkPtr_t weak);
234
235 private:
244 NodePtr_t addNode(ConfigurationIn_t config,
245 ConnectedComponentPtr_t connectedComponent);
246
250 void connect(const ConnectedComponentPtr_t& cc1,
251 const ConnectedComponentPtr_t& cc2);
252
256 void merge(const ConnectedComponentPtr_t& cc1,
257 ConnectedComponent::RawPtrs_t& ccs);
258
259 const DistancePtr_t distance_;
260 path::CostPtr_t cost_;
261 ConnectedComponents_t connectedComponents_;
262 Nodes_t nodes_;
263 Edges_t edges_;
264 NodePtr_t initNode_;
265 NodeVector_t goalNodes_;
266 NearestNeighborPtr_t nearestNeighbor_;
267 RoadmapWkPtr_t weak_;
268
269 HPP_SERIALIZABLE();
270}; // class Roadmap
271std::ostream& operator<<(std::ostream& os, const Roadmap& r);
273} // namespace core
274} // namespace hpp
275#endif // HPP_CORE_ROADMAP_HH
hpp::core::ConnectedComponent::RawPtrs_t
std::set< RawPtr_t > RawPtrs_t
Definition connected-component.hh:46
hpp::core::Edge
Definition edge.hh:46
hpp::core::Node
Definition node.hh:46
hpp::core::Roadmap
Definition roadmap.hh:46
hpp::core::Roadmap::nearestNode
NodePtr_t nearestNode(ConfigurationIn_t configuration, const ConnectedComponentPtr_t &connectedComponent, value_type &minDistance, bool reverse=false)
hpp::core::Roadmap::nearestNodes
Nodes_t nearestNodes(ConfigurationIn_t configuration, size_type k)
hpp::core::Roadmap::insertPathVector
void insertPathVector(const PathVectorPtr_t &path, bool backAndForth)
hpp::core::Roadmap::initNode
NodePtr_t initNode() const
Definition roadmap.hh:177
hpp::core::Roadmap::createNode
virtual NodePtr_t createNode(ConfigurationIn_t configuration) const
hpp::core::Roadmap::nearestNodes
Nodes_t nearestNodes(ConfigurationIn_t configuration, const ConnectedComponentPtr_t &connectedComponent, size_type k)
hpp::core::Roadmap::addGoalNode
NodePtr_t addGoalNode(ConfigurationIn_t config)
hpp::core::Roadmap::edges
const Edges_t & edges() const
Definition roadmap.hh:176
hpp::core::Roadmap::init
void init(RoadmapWkPtr_t weak)
Store weak pointer to itself.
hpp::core::Roadmap::pathExists
bool pathExists() const
Check that a path exists between the initial node and one goal node.
hpp::core::Roadmap::addEdges
void addEdges(const NodePtr_t from, const NodePtr_t &to, const PathPtr_t &path)
hpp::core::Roadmap::addEdge
EdgePtr_t addEdge(const NodePtr_t &n1, const NodePtr_t &n2, const PathPtr_t &path)
Add an edge between two nodes.
hpp::core::Roadmap::connectedComponents
const ConnectedComponents_t & connectedComponents() const
Get list of connected component of the roadmap.
hpp::core::Roadmap::nodesWithinBall
NodeVector_t nodesWithinBall(ConfigurationIn_t configuration, const ConnectedComponentPtr_t &connectedComponent, value_type maxDistance)
hpp::core::Roadmap::addConnectedComponent
void addConnectedComponent(const NodePtr_t &node)
hpp::core::Roadmap::initNode
void initNode(ConfigurationIn_t config)
Definition roadmap.hh:170
hpp::core::Roadmap::resetGoalNodes
void resetGoalNodes()
Definition roadmap.hh:168
hpp::core::Roadmap::goalNodes
const NodeVector_t & goalNodes() const
Definition roadmap.hh:178
hpp::core::Roadmap::addNodeAndEdge
NodePtr_t addNodeAndEdge(const NodePtr_t from, ConfigurationIn_t to, const PathPtr_t path)
hpp::core::Roadmap::addNodeAndEdges
NodePtr_t addNodeAndEdges(const NodePtr_t from, ConfigurationIn_t to, const PathPtr_t path)
hpp::core::Roadmap::distance
const DistancePtr_t & distance() const
hpp::core::Roadmap::push_node
virtual void push_node(const NodePtr_t &n)
Definition roadmap.hh:214
hpp::core::Roadmap::print
std::ostream & print(std::ostream &os) const
hpp::core::Roadmap::impl_addEdge
virtual void impl_addEdge(const EdgePtr_t &e)
hpp::core::Roadmap::merge
void merge(const RoadmapPtr_t &other)
Add the nodes and edges of a roadmap into this one.
hpp::core::Roadmap::addNodeAndEdge
NodePtr_t addNodeAndEdge(ConfigurationIn_t from, const NodePtr_t to, const PathPtr_t path)
hpp::core::Roadmap::nearestNeighbor
NearestNeighborPtr_t nearestNeighbor()
Get nearestNeighbor object.
hpp::core::Roadmap::nodes
const Nodes_t & nodes() const
Definition roadmap.hh:175
hpp::core::Roadmap::nearestNeighbor
void nearestNeighbor(NearestNeighborPtr_t nearestNeighbor)
Set new NearestNeighbor (roadmap must be empty)
hpp::core::Roadmap::clear
virtual void clear()
Clear the roadmap by deleting nodes and edges.
hpp::core::Roadmap::Roadmap
Roadmap()
Definition roadmap.hh:205
hpp::core::Roadmap::create
static RoadmapPtr_t create(const DistancePtr_t &distance, const DevicePtr_t &robot)
Return shared pointer to new instance.
hpp::core::Roadmap::addNode
NodePtr_t addNode(ConfigurationIn_t config)
hpp::core::Roadmap::cost
void cost(const path::CostPtr_t &cost)
Set cost used to search for optimal path.
hpp::core::Roadmap::cost
const path::CostPtr_t cost() const
Get cost used to search for optimal path.
hpp::core::Roadmap::nearestNode
NodePtr_t nearestNode(ConfigurationIn_t configuration, value_type &minDistance, bool reverse=false)
hpp::core::Roadmap::Roadmap
Roadmap(const DistancePtr_t &distance, const DevicePtr_t &robot)
hpp::core::Roadmap::~Roadmap
virtual ~Roadmap()
config.hh
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition config.hh:88
connected-component.hh
fwd.hh
hpp::core::operator<<
std::ostream & operator<<(std::ostream &os, const Constraint &constraint)
Definition constraint.hh:99
hpp
hpp::core::path::CostPtr_t
shared_ptr< Cost > CostPtr_t
Definition fwd.hh:249
hpp::core::value_type
pinocchio::value_type value_type
Definition fwd.hh:174
hpp::core::PathVectorPtr_t
shared_ptr< PathVector > PathVectorPtr_t
Definition fwd.hh:193
hpp::core::DistancePtr_t
shared_ptr< Distance > DistancePtr_t
Definition fwd.hh:141
hpp::core::NodeVector_t
std::vector< NodePtr_t > NodeVector_t
Definition fwd.hh:182
hpp::core::ConnectedComponents_t
std::set< ConnectedComponentPtr_t, SharedComparator > ConnectedComponents_t
Definition fwd.hh:127
hpp::core::Edges_t
std::list< Edge * > Edges_t
Definition fwd.hh:145
hpp::core::RoadmapPtr_t
shared_ptr< Roadmap > RoadmapPtr_t
Definition fwd.hh:199
hpp::core::NearestNeighborPtr_t
shared_ptr< NearestNeighbor > NearestNeighborPtr_t
Definition fwd.hh:281
hpp::core::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:108
hpp::core::size_type
pinocchio::size_type size_type
Definition fwd.hh:173
hpp::core::Nodes_t
std::list< NodePtr_t > Nodes_t
Definition fwd.hh:181
hpp::core::ConnectedComponentPtr_t
shared_ptr< ConnectedComponent > ConnectedComponentPtr_t
Definition fwd.hh:117
hpp::core::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
Definition fwd.hh:134
hpp::core::PathPtr_t
shared_ptr< Path > PathPtr_t
Definition fwd.hh:187
serialization-fwd.hh
HPP_SERIALIZABLE
#define HPP_SERIALIZABLE()