hpp-manipulation 4.15.1
Classes for manipulation planning.
Loading...
Searching...
No Matches
edge.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_EDGE_HH
30#define HPP_MANIPULATION_GRAPH_EDGE_HH
31
32#include <hpp/core/constraint-set.hh>
33#include <hpp/core/path.hh>
34#include <hpp/core/relative-motion.hh>
35#include <hpp/core/steering-method.hh>
36
40
41namespace hpp {
42namespace manipulation {
43namespace graph {
46
50
72 friend class WaypointEdge;
73
74 public:
75 typedef core::RelativeMotion RelativeMotion;
76
78 virtual ~Edge();
79
81 static EdgePtr_t create(const std::string& name, const GraphWkPtr_t& graph,
82 const StateWkPtr_t& from, const StateWkPtr_t& to);
83
93 virtual bool applyConstraints(core::NodePtr_t nStart, ConfigurationOut_t q)
95
107
116 virtual bool generateTargetConfig(core::NodePtr_t nStart,
117 ConfigurationOut_t q) const;
118
128 ConfigurationOut_t q) const;
129
131
132 virtual bool build(core::PathPtr_t& path, ConfigurationIn_t q1,
133 ConfigurationIn_t q2) const;
134
137
140
142 StatePtr_t state() const { return state_.lock(); }
143
144 void state(StatePtr_t state) { state_ = state; }
145
147 const core::SteeringMethodPtr_t& steeringMethod() const {
148 return steeringMethod_;
149 }
150
152 const core::PathValidationPtr_t& pathValidation() const {
153 return pathValidation_;
154 }
155
156 const RelativeMotion::matrix_type& relativeMotion() const {
157 return relMotion_;
158 }
159
161 void relativeMotion(const RelativeMotion::matrix_type& m);
162
171 void securityMarginForPair(const size_type& row, const size_type& col,
172 const value_type& margin);
173
175 const matrix_t& securityMargins() const { return securityMargins_; }
176
179 virtual bool direction(const core::PathPtr_t& path) const;
180
183 virtual bool intersectionConstraint(const EdgePtr_t& other,
184 ConfigProjectorPtr_t projector) const;
185
187 virtual std::ostream& dotPrint(
188 std::ostream& os,
190
194
196 ConstraintSetPtr_t targetConstraint() const;
197
198 void setShort(bool isShort) { isShort_ = isShort; }
199
200 bool isShort() const { return isShort_; }
204
205 protected:
207 void init(const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
208 const StateWkPtr_t& from, const StateWkPtr_t& to);
209
211 Edge(const std::string& name);
212
215
217 virtual ConstraintSetPtr_t buildTargetConstraint();
218
220 virtual ConstraintSetPtr_t buildPathConstraint();
221
222 virtual void initialize();
223
225 virtual std::ostream& print(std::ostream& os) const;
226
227 bool isShort_;
228
229 private:
231 ConstraintSetPtr_t pathConstraints_;
232
235 ConstraintSetPtr_t targetConstraints_;
236
238 StateWkPtr_t from_, to_;
239
241 StateWkPtr_t state_;
242
244 core::SteeringMethodPtr_t steeringMethod_;
245
247 mutable RelativeMotion::matrix_type relMotion_;
249 matrix_t securityMargins_;
250
251 core::PathValidationPtr_t pathValidation_;
252
254 EdgeWkPtr_t wkPtr_;
255
256 friend class Graph;
257}; // class Edge
258
294 public:
296 static WaypointEdgePtr_t create(const std::string& name,
297 const GraphWkPtr_t& graph,
298 const StateWkPtr_t& from,
299 const StateWkPtr_t& to);
300
302
303 virtual bool build(core::PathPtr_t& path, ConfigurationIn_t q1,
304 ConfigurationIn_t q2) const;
305
317
327 ConfigurationOut_t q) const;
328
330 const EdgePtr_t& waypoint(const std::size_t index) const;
331
333 virtual std::ostream& dotPrint(
334 std::ostream& os,
336
338 void nbWaypoints(const size_type number);
339
340 std::size_t nbWaypoints() const { return edges_.size() - 1; }
341
344 void setWaypoint(const std::size_t index, const EdgePtr_t wEdge,
345 const StatePtr_t wTo);
346
347 protected:
348 WaypointEdge(const std::string& name) : Edge(name), lastSucceeded_(false) {}
350 void init(const WaypointEdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
351 const StateWkPtr_t& from, const StateWkPtr_t& to);
352
354 virtual void initialize();
356 virtual std::ostream& print(std::ostream& os) const;
357
358 private:
359 Edges_t edges_;
360 States_t states_;
361
362 mutable matrix_t configs_;
363 mutable bool lastSucceeded_;
364
365 WaypointEdgeWkPtr_t wkPtr_;
366}; // class WaypointEdge
367
442 public:
443 virtual ~LevelSetEdge();
444
446 static LevelSetEdgePtr_t create(const std::string& name,
447 const GraphWkPtr_t& graph,
448 const StateWkPtr_t& from,
449 const StateWkPtr_t& to);
450
460 virtual bool applyConstraints(core::NodePtr_t nStart, ConfigurationOut_t q)
462
474
483 virtual bool generateTargetConfig(core::NodePtr_t nStart,
484 ConfigurationOut_t q) const;
485
495 ConfigurationOut_t q) const;
496
505 ConfigurationIn_t qLeaf,
506 ConfigurationOut_t q) const;
507
511
513 virtual ConstraintSetPtr_t buildTargetConstraint();
514
517 void buildHistogram();
518
535 LeafHistogramPtr_t histogram() const;
536
539
542 void insertParamConstraint(const ImplicitPtr_t& nm);
543
545 const NumericalConstraints_t& paramConstraints() const;
546
549 void insertConditionConstraint(const ImplicitPtr_t& nm);
550
553 const NumericalConstraints_t& conditionConstraints() const;
555
557 virtual std::ostream& dotPrint(
558 std::ostream& os,
559 dot::DrawingAttributes da = dot::DrawingAttributes()) const;
560
561 protected:
563 void init(const LevelSetEdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
564 const StateWkPtr_t& from, const StateWkPtr_t& to);
565
566 LevelSetEdge(const std::string& name);
567
569 virtual std::ostream& print(std::ostream& os) const;
570
572 virtual void populateTooltip(dot::Tooltip& tp) const;
573
574 virtual void initialize();
575
576 private:
577 // Parametrizer
578 // NumericalConstraints_t
579 NumericalConstraints_t paramNumericalConstraints_;
580
581 // Condition
582 // NumericalConstraints_t
583 NumericalConstraints_t condNumericalConstraints_;
584
586 LeafHistogramPtr_t hist_;
587
588 LevelSetEdgeWkPtr_t wkPtr_;
589}; // class LevelSetEdge
590
592} // namespace graph
593} // namespace manipulation
594} // namespace hpp
595
596#endif // HPP_MANIPULATION_GRAPH_EDGE_HH
Definition: edge.hh:71
const matrix_t & securityMargins() const
Accessor to the security margin.
Definition: edge.hh:175
const RelativeMotion::matrix_type & relativeMotion() const
Definition: edge.hh:156
StatePtr_t stateFrom() const
Get the origin.
virtual ConstraintSetPtr_t buildConfigConstraint() HPP_MANIPULATION_DEPRECATED
bool isShort() const
Definition: edge.hh:200
void relativeMotion(const RelativeMotion::matrix_type &m)
Update the relative motion matrix.
virtual ~Edge()
Destructor.
static EdgePtr_t create(const std::string &name, const GraphWkPtr_t &graph, const StateWkPtr_t &from, const StateWkPtr_t &to)
Create a new empty Edge.
const core::SteeringMethodPtr_t & steeringMethod() const
Get steering method associated to the edge.
Definition: edge.hh:147
virtual bool build(core::PathPtr_t &path, ConfigurationIn_t q1, ConfigurationIn_t q2) const
const core::PathValidationPtr_t & pathValidation() const
Get path validation associated to the edge.
Definition: edge.hh:152
virtual bool intersectionConstraint(const EdgePtr_t &other, ConfigProjectorPtr_t projector) const
StatePtr_t stateTo() const
Get the destination.
virtual bool generateTargetConfig(ConfigurationIn_t qStart, ConfigurationOut_t q) const
ConstraintSetPtr_t pathConstraint() const
Edge(const std::string &name)
Constructor.
virtual bool generateTargetConfig(core::NodePtr_t nStart, ConfigurationOut_t q) const
virtual std::ostream & dotPrint(std::ostream &os, dot::DrawingAttributes da=dot::DrawingAttributes()) const
Print the object in a stream.
virtual bool applyConstraints(ConfigurationIn_t qStart, ConfigurationOut_t q) const HPP_MANIPULATION_DEPRECATED
void securityMarginForPair(const size_type &row, const size_type &col, const value_type &margin)
ConstraintSetPtr_t configConstraint() const HPP_MANIPULATION_DEPRECATED
virtual bool canConnect(ConfigurationIn_t q1, ConfigurationIn_t q2) const
virtual bool direction(const core::PathPtr_t &path) const
void init(const EdgeWkPtr_t &weak, const GraphWkPtr_t &graph, const StateWkPtr_t &from, const StateWkPtr_t &to)
Initialization of the object.
StatePtr_t state() const
Get the state in which path is.
Definition: edge.hh:142
void state(StatePtr_t state)
Definition: edge.hh:144
virtual bool applyConstraints(core::NodePtr_t nStart, ConfigurationOut_t q) const HPP_MANIPULATION_DEPRECATED
core::RelativeMotion RelativeMotion
Definition: edge.hh:75
Define common methods of the graph components.
Definition: graph-component.hh:51
Definition: graph.hh:58
virtual ConstraintSetPtr_t buildConfigConstraint() HPP_MANIPULATION_DEPRECATED
virtual bool generateTargetConfig(ConfigurationIn_t qStart, ConfigurationOut_t q) const
virtual bool applyConstraints(ConfigurationIn_t qStart, ConfigurationOut_t q) const HPP_MANIPULATION_DEPRECATED
virtual bool generateTargetConfig(core::NodePtr_t nStart, ConfigurationOut_t q) const
bool generateTargetConfigOnLeaf(ConfigurationIn_t qStart, ConfigurationIn_t qLeaf, ConfigurationOut_t q) const
virtual bool applyConstraints(core::NodePtr_t nStart, ConfigurationOut_t q) const HPP_MANIPULATION_DEPRECATED
static LevelSetEdgePtr_t create(const std::string &name, const GraphWkPtr_t &graph, const StateWkPtr_t &from, const StateWkPtr_t &to)
Create a new LevelSetEdge.
virtual bool applyConstraints(ConfigurationIn_t qStart, ConfigurationOut_t q) const HPP_MANIPULATION_DEPRECATED
virtual bool canConnect(ConfigurationIn_t q1, ConfigurationIn_t q2) const
void nbWaypoints(const size_type number)
Set the number of waypoints.
virtual std::ostream & print(std::ostream &os) const
Print the object in a stream.
void setWaypoint(const std::size_t index, const EdgePtr_t wEdge, const StatePtr_t wTo)
std::size_t nbWaypoints() const
Definition: edge.hh:340
virtual bool generateTargetConfig(ConfigurationIn_t qStart, ConfigurationOut_t q) const
WaypointEdge(const std::string &name)
Definition: edge.hh:348
virtual void initialize()
Initialize each of the internal edges.
virtual bool build(core::PathPtr_t &path, ConfigurationIn_t q1, ConfigurationIn_t q2) const
static WaypointEdgePtr_t create(const std::string &name, const GraphWkPtr_t &graph, const StateWkPtr_t &from, const StateWkPtr_t &to)
Create a new WaypointEdge.
void init(const WaypointEdgeWkPtr_t &weak, const GraphWkPtr_t &graph, const StateWkPtr_t &from, const StateWkPtr_t &to)
Initialization of the object.
const EdgePtr_t & waypoint(const std::size_t index) const
Return the index-th edge.
virtual std::ostream & dotPrint(std::ostream &os, dot::DrawingAttributes da=dot::DrawingAttributes()) const
Print the object in a stream.
#define HPP_MANIPULATION_DLLAPI
Definition: config.hh:64
#define HPP_MANIPULATION_DEPRECATED
Definition: deprecated.hh:37
hpp::core::NumericalConstraints_t NumericalConstraints_t
Definition: fwd.hh:64
shared_ptr< Edge > EdgePtr_t
Definition: fwd.hh:49
shared_ptr< State > StatePtr_t
Definition: fwd.hh:48
shared_ptr< LevelSetEdge > LevelSetEdgePtr_t
Definition: fwd.hh:51
std::vector< EdgePtr_t > Edges_t
Definition: fwd.hh:57
StateHistogram NodeHistogram HPP_MANIPULATION_DEPRECATED
Definition: statistics.hh:206
std::vector< StatePtr_t > States_t
Definition: fwd.hh:56
shared_ptr< WaypointEdge > WaypointEdgePtr_t
Definition: fwd.hh:50
shared_ptr< LeafHistogram > LeafHistogramPtr_t
Definition: fwd.hh:72
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:123
core::PathValidationPtr_t PathValidationPtr_t
Definition: fwd.hh:129
constraints::ImplicitPtr_t ImplicitPtr_t
Definition: fwd.hh:116
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:50
core::ConfigProjectorPtr_t ConfigProjectorPtr_t
Definition: fwd.hh:121
core::size_type size_type
Definition: fwd.hh:91
core::value_type value_type
Definition: fwd.hh:90
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:49
core::matrix_t matrix_t
Definition: fwd.hh:131
shared_ptr< SteeringMethod > SteeringMethodPtr_t
Definition: fwd.hh:101
Definition: main.hh:1