hpp-manipulation 4.14.0
Classes for manipulation planning.
Loading...
Searching...
No Matches
graph.hh
Go to the documentation of this file.
1// Copyright (c) 2014, LAAS-CNRS
2// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3//
4
5// Redistribution and use in source and binary forms, with or without
6// modification, are permitted provided that the following conditions are
7// met:
8//
9// 1. Redistributions of source code must retain the above copyright
10// notice, this list of conditions and the following disclaimer.
11//
12// 2. Redistributions in binary form must reproduce the above copyright
13// notice, this list of conditions and the following disclaimer in the
14// documentation and/or other materials provided with the distribution.
15//
16// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27// DAMAGE.
28
29#ifndef HPP_MANIPULATION_GRAPH_GRAPH_HH
30#define HPP_MANIPULATION_GRAPH_GRAPH_HH
31
32#include <tuple>
33
39
40namespace hpp {
41namespace manipulation {
42typedef constraints::ImplicitPtr_t ImplicitPtr_t;
43namespace graph {
46
59 public:
64 static GraphPtr_t create(const std::string& name, DevicePtr_t robot,
65 const ProblemPtr_t& problem);
66
67 GraphPtr_t self() const { return wkPtr_.lock(); }
68
70 StateSelectorPtr_t createStateSelector(const std::string& name);
71
77
79 StateSelectorPtr_t stateSelector() const { return stateSelector_; }
80
83
86
88 Edges_t getEdges(const StatePtr_t& from, const StatePtr_t& to) const;
89
92
96
110 void registerConstraints(const ImplicitPtr_t& constraint,
111 const ImplicitPtr_t& complement,
112 const ImplicitPtr_t& both);
113
125 bool isComplement(const ImplicitPtr_t& constraint,
126 const ImplicitPtr_t& complement,
127 ImplicitPtr_t& combinationOfBoth) const;
128
134
139
144
152
163 vector_t& error) const;
164
172 // of the edge
177 vector_t& error) const;
178
190 ConfigurationIn_t config,
191 const EdgePtr_t& edge, vector_t& error) const;
192
204 ConfigurationIn_t config,
205 const EdgePtr_t& edge,
206 vector_t& error) const;
207
212
214 void maxIterations(size_type iterations);
215
218
220 void errorThreshold(const value_type& threshold);
221
224
226 const DevicePtr_t& robot() const;
227
229 const ProblemPtr_t& problem() const;
230
232 void problem(const ProblemPtr_t& problem);
233
236 hists_.push_back(hist);
237 }
238
240 const Histograms_t& histograms() const { return hists_; }
241
243 GraphComponentWkPtr_t get(std::size_t id) const;
244
245 std::size_t nbComponents() const { return components_.size(); }
246
248 virtual std::ostream& dotPrint(
249 std::ostream& os,
251
253 virtual void initialize();
254
256 virtual void invalidate();
257
258 protected:
260 void init(const GraphWkPtr_t& weak, DevicePtr_t robot);
261
264 Graph(const std::string& name, const ProblemPtr_t& problem);
265
267 std::ostream& print(std::ostream& os) const;
268
269 private:
271 GraphComponents_t& components();
272
274 GraphComponents_t components_;
275
277 StateSelectorPtr_t stateSelector_;
278
281 ConstraintPtr_t constraints_;
282
284 DevicePtr_t robot_;
285
287 GraphWkPtr_t wkPtr_;
288
290 typedef std::map<StatePtr_t, ConstraintSetPtr_t> MapFromState;
291 typedef std::pair<StatePtr_t, ConstraintSetPtr_t> PairStateConstraints;
292 MapFromState constraintSetMapFromState_;
293
295 Histograms_t hists_;
296
298 typedef std::map<EdgePtr_t, ConstraintSetPtr_t> MapFromEdge;
299 typedef std::pair<EdgePtr_t, ConstraintSetPtr_t> PairEdgeConstraints;
300 MapFromEdge cfgConstraintSetMapFromEdge_, pathConstraintSetMapFromEdge_;
301 ProblemPtr_t problem_;
302 value_type errorThreshold_;
303 size_type maxIterations_;
304
305 ConstraintsAndComplements_t constraintsAndComplements_;
306 friend class GraphComponent;
307}; // Class Graph
308
310} // namespace graph
311} // namespace manipulation
312
313} // namespace hpp
314
315BOOST_CLASS_EXPORT_KEY(hpp::manipulation::graph::Graph)
316
317#endif // HPP_MANIPULATION_GRAPH_GRAPH_HH
Definition: roadmap-node.hh:42
Define common methods of the graph components.
Definition: graph-component.hh:51
Definition: graph.hh:58
void stateSelector(StateSelectorPtr_t ns)
StateSelectorPtr_t createStateSelector(const std::string &name)
Create and insert a state selector inside the graph.
value_type errorThreshold() const
Get error threshold in config projector.
const ConstraintsAndComplements_t & constraintsAndComplements() const
const ProblemPtr_t & problem() const
Get the problem.
void errorThreshold(const value_type &threshold)
Set error threshold.
virtual void initialize()
Initialize all components of the graph (edges and states)
bool getConfigErrorForEdge(ConfigurationIn_t config, const EdgePtr_t &edge, vector_t &error) const
void problem(const ProblemPtr_t &problem)
Set the problem.
Edges_t getEdges(const StatePtr_t &from, const StatePtr_t &to) const
Get possible edges between two nodes.
GraphComponentWkPtr_t get(std::size_t id) const
Get the component by its ID.
void insertHistogram(const graph::HistogramPtr_t &hist)
Register an histogram representing a foliation.
Definition: graph.hh:235
ConstraintSetPtr_t configConstraint(const StatePtr_t &state) const
StatePtr_t getState(RoadmapNodePtr_t node) const
Returns the state of a roadmap node.
static GraphPtr_t create(const std::string &name, DevicePtr_t robot, const ProblemPtr_t &problem)
Graph(const std::string &name, const ProblemPtr_t &problem)
void maxIterations(size_type iterations)
Set maximal number of iterations.
const Histograms_t & histograms() const
Get the histograms.
Definition: graph.hh:240
void registerConstraints(const ImplicitPtr_t &constraint, const ImplicitPtr_t &complement, const ImplicitPtr_t &both)
size_type maxIterations() const
Get maximal number of iterations in config projector.
const DevicePtr_t & robot() const
Get the robot.
ConstraintSetPtr_t targetConstraint(const EdgePtr_t &edge) const
virtual void invalidate()
Invalidate all states and edges of the graph.
StateSelectorPtr_t stateSelector() const
Get the state selector.
Definition: graph.hh:79
virtual std::ostream & dotPrint(std::ostream &os, dot::DrawingAttributes da=dot::DrawingAttributes()) const
Print the component in DOT language.
bool getConfigErrorForEdgeLeaf(ConfigurationIn_t leafConfig, ConfigurationIn_t config, const EdgePtr_t &edge, vector_t &error) const
bool getConfigErrorForState(ConfigurationIn_t config, const StatePtr_t &state, vector_t &error) const
GraphPtr_t self() const
Definition: graph.hh:67
std::size_t nbComponents() const
Definition: graph.hh:245
std::ostream & print(std::ostream &os) const
Print the object in a stream.
ConstraintSetPtr_t configConstraint(const EdgePtr_t &edge) const HPP_MANIPULATION_DEPRECATED
void init(const GraphWkPtr_t &weak, DevicePtr_t robot)
Initialization of the object.
EdgePtr_t chooseEdge(RoadmapNodePtr_t node) const
Select randomly outgoing edge of the given node.
ConstraintSetPtr_t pathConstraint(const EdgePtr_t &edge) const
bool isComplement(const ImplicitPtr_t &constraint, const ImplicitPtr_t &complement, ImplicitPtr_t &combinationOfBoth) const
StatePtr_t getState(ConfigurationIn_t config) const
Returns the state of a configuration.
bool getConfigErrorForEdgeTarget(ConfigurationIn_t leafConfig, ConfigurationIn_t config, const EdgePtr_t &edge, vector_t &error) const
#define HPP_MANIPULATION_DLLAPI
Definition: config.hh:64
#define HPP_MANIPULATION_DEPRECATED
Definition: deprecated.hh:37
std::vector< ConstraintAndComplement_t > ConstraintsAndComplements_t
Definition: constraint-set.hh:87
shared_ptr< Edge > EdgePtr_t
Definition: fwd.hh:49
shared_ptr< State > StatePtr_t
Definition: fwd.hh:48
std::vector< EdgePtr_t > Edges_t
Definition: fwd.hh:57
shared_ptr< StateSelector > StateSelectorPtr_t
Definition: fwd.hh:52
std::list< HistogramPtr_t > Histograms_t
Definition: fwd.hh:73
shared_ptr< Histogram > HistogramPtr_t
Definition: fwd.hh:70
std::vector< GraphComponentWkPtr_t > GraphComponents_t
Definition: fwd.hh:55
shared_ptr< Graph > GraphPtr_t
Definition: fwd.hh:47
shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:66
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:123
constraints::ImplicitPtr_t ImplicitPtr_t
Definition: fwd.hh:116
core::size_type size_type
Definition: fwd.hh:91
core::value_type value_type
Definition: fwd.hh:90
shared_ptr< Device > DevicePtr_t
Definition: fwd.hh:40
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:49
core::vector_t vector_t
Definition: fwd.hh:93
core::ConstraintPtr_t ConstraintPtr_t
Definition: fwd.hh:113
Definition: main.hh:1